%extendedKalmanFilter Extended Kalman filter
%   Estimate states of a discrete-time nonlinear model using Extended Kalman
%   filter algorithm. The model may have one of the following two forms:
%
%   Additive noise models:
%      x[k] = f(x[k-1], u1[k-1]) + w[k-1]  (state transition equation)
%      y[k] = h(x[k], u2[k]) + v[k]        (measurement equation)
%
%   Non-additive noise models:
%      x[k] = f(x[k-1], w[k-1], u1[k-1])   (state transition equation)
%      y[k] = h(x[k], v[k], u2[k])         (measurement equation)
%
%   x is the state, w is the process noise, v is the measurement noise, 
%   k is the time. u1 and u2 are the (optional) inputs. f(...) and h(...)
%   are the state transition and measurement functions, respectively. 
%
%   The extended Kalman filter relies on first-order Taylor series
%   approximations (Jacobians) to the state-transition and measurement
%   functions. The following Jacobians are evaluated at the current state
%   estimate:
%
%       Jfx = df/dx                            (state Jacobian)
%       Jhx = dh/dx                            (measurement Jacobian)
%
%   If the noise models are non-additive, the Jacobians df/dw and dh/dv are
%   also evaluated.
%
%   extendedKalmanFilter creates an object for state estimation. Use the
%   predict and correct methods with this object to perform estimation.
%      * Predict: Use the previous states to predict the current state.
%      * Correct: Use the current measurement to correct (or update) the state.
%
%   obj = extendedKalmanFilter returns an extended Kalman filter object with
%   default state transition function and measurement function. It assumes
%   additive noise models.
%
%   obj = extendedKalmanFilter(StateTransitionFcn, MeasurementFcn, State)
%   lets you specify the state transition function, f, and the measurement
%   model, h. Both must be a function_handle. In addition, it lets you
%   specify an initial state estimate.
%
%   obj = extendedKalmanFilter(..., Name, Value) configures the
%   extended Kalman filter object properties, specified as one or more
%   name-value pair arguments. Unspecified properties have default values.
%
%   predict method syntax:
%
%   [x_pred, P_pred] = predict(obj) returns the prediction of state and
%   state estimation error covariance at the next time step. The State and
%   StateCovariance properties of the extended Kalman filter are
%   overwritten with the prediction results.
%
%   [x_pred, P_pred] = predict(obj, varargin) additionally, lets you
%   specify additional inputs that will be passed to and used by the
%   StateTransitionFcn, f. For instance, you can use this syntax to pass a
%   known system input, u, to the StateTransitionFcn.
%
%   correct method syntax:
%
%   [x_corr, P_corr] = correct(obj, y) returns the corrected state and
%   state estimation error covariance based on the current measurement y.
%   The State and StateCovariance properties of the extended Kalman filter
%   are overwritten with the corrected results.
%
%   [x_corr, P_corr] = correct(obj, y, varargin) additionally, lets you
%   specify input arguments that will be passed to and used by the
%   MeasurementFcn. For instance, you can use this syntax to pass a known
%   system input, u, to the MeasurementFcn.
%
%   Notes:
%   ======
%   * If the measurement exists, 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.
%
%       If there is measurement at current time step
%          predict(extendedKalmanFilter);
%          stateEstimate = correct(extendedKalmanFilter, objectLocation);
%       Else
%          stateEstimate = predict(extendedKalmanFilter);
%       End
%
%
%   extendedKalmanFilter methods:
%
%   predict  - Predicts the state and state estimation error covariance
%   correct  - Corrects the state and state estimation error covariance
%   clone    - Creates a copy of the object with the same property values
%   residual - Returns measurement residual and residual covariance
%   generateJacobianFcn - Generate Jacobians using auto-differentiation techniques
%
%   extendedKalmanFilter properties:
%
%   HasAdditiveProcessNoise     - True if process noise is additive
%   StateTransitionFcn          - Promotes the state to next time step, (f)
%   HasAdditiveMeasurementNoise - True if measurement noise is additive
%   MeasurementFcn              - Calculates the measurement, (h)
%   State                       - State, (x)
%   StateCovariance             - State estimation error covariance, (P)
%   ProcessNoise                - Process noise covariance, (Q)
%   MeasurementNoise            - Measurement noise covariance, (R)
%   StateTransitionJacobianFcn  - State transition Jacobian matrix, (df/dx, df/dw)
%   MeasurementJacobianFcn      - Measurement Jacobian matrix, (dh/dx, dh/dv)
%
%   % Examples:
%
%   %% Example1: Create and use the filter
%   % Generate data: Simulate the van der Pol ODEs for mu = 1 to get the true states
%   dt = 0.05; % [s] Filter sample time
%   tSpan = 0:dt:5;
%   [~,xTrue]=ode45(@vdp1,tSpan,[2;0]);
%   % Get measurements: A sensor measures the first state plus noise
%   sqrtR = 0.04; % Standard deviation of measurement noise
%   yMeas = xTrue(:,1) + sqrtR*randn(numel(tSpan),1);
%   %% Construct filter
%   initialStateGuess = [2; 0];
%   myEKF = extendedKalmanFilter(@vdpStateFcn,@vdpMeasurementFcn,initialStateGuess);
%   myEKF.ProcessNoise = 0.01; % Variance of the process noise
%   myEKF.MeasurementNoise = sqrtR^2; % Variance of the measurement noise
%   %% Estimate 2 states from noisy measurement of the first state
%   xEst = zeros(size(xTrue));
%   for k=1:size(xTrue,1)
%      % Use measurement y[k] to correct the state estimate for time k
%      xEst(k,:) = correct(myEKF,yMeas(k)); 
%      % The result is x[k|k]: Estimate of states at time k, utilizing
%      % measurements up to time k. This is also stored in the State 
%      % property of the filter.
%      %
%      % Now, predict states at next time step. This updates the State and 
%      % StateCovariance properties of the filter. These are utilized in
%      % the next correct command
%      predict(myEKF); % Filter stores x[k+1|k], P[k+1|k]
%   end
%   %% Plot results
%   plot(xTrue(:,1),xTrue(:,2),'x',xEst(:,1),xEst(:,2),'ro');
%   legend('True','Estimated');
%
%   %% Example2: Non-additive measurement noise
%   % The relationship between the measurement noise v[k] and y[k] is
%   % nonlinear. Specify this when creating the filter. The first two
%   % inputs of the measurement function must correspond to x[k] and v[k]
%   myEKF = extendedKalmanFilter(@vdpStateFcn, ...
%                    @vdpMeasurementNonAdditiveNoiseFcn, [2;0],...
%                    'HasAdditiveMeasurementNoise', false());
%
%   %% Example3: Pass additional inputs to state and measurement functions
%   % Assume the state transition model is x[k+1] = sqrt(x[k]) + u[k] + w[k], 
%   % and the measurement model is y[k] = x[k] + 2*u[k] + v[k]. Here both
%   % the process and measurement noise terms are additive. Add the extra 
%   % input u in the state-transition and measurement functions after state x.
%   f = @(x,u)(sqrt(x)+u); % Extra input u[k] follows x[k]
%   h = @(x,u)(x+2*u);
%   myEKF = extendedKalmanFilter(f,h,0); % 0 is the initial state guess
%   %% Provide extra inputs through correct and predict
%   % correct the state estimate with measurement y[k]=0.8, input u[k]=0.2
%   correct(myEKF,0.8,0.2)
%   % predict the states at next time, given u[k]=0.2
%   predict(myEKF,0.2)
%
%   %% Example4: Specify Jacobian functions
%   % If available, provide the Jacobians of the state-transition and
%   % measurement functions to improve estimation accuracy and speed. 
%   myEKF = extendedKalmanFilter(@vdpStateFcn, @vdpMeasurementFcn, [2;0],...
%                    'StateTransitionJacobianFcn', @vdpStateJacobianFcn,...
%                    'MeasurementJacobianFcn',     @vdpMeasurementJacobianFcn);
%  % If the Jacobian functions are not available you can generate them using 
%  % auto-differentiation techniques.
%  myEKF.StateTransitionJacobianFcn = generateJacobianFcn(myEKF, 'state');
%  myEKF.MeasurementJacobianFcn = generateJacobianFcn(myEKF, 'measurement');
%
%  See also unscentedKalmanFilter, particleFilter, kalman, kalmd.

%   Copyright 2016-2023 The MathWorks, Inc.

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

classdef extendedKalmanFilter < matlabshared.tracking.internal.ExtendedKalmanFilter
    methods
        function EKF = extendedKalmanFilter(varargin)
            controllib.internal.util.initializeFilter();
            EKF@matlabshared.tracking.internal.ExtendedKalmanFilter(varargin{:});
            if nargin==0
                EKF.StateTransitionFcn = @vdpStateFcn;
                EKF.MeasurementFcn = @vdpMeasurementFcn;
                EKF.State = [0;0];
            end           
        end
    end
    
    methods (Static = true)
        function retEKF = loadobj(sobj)
            
            % Cannot inherit from superclass because the static method does
            % not know the correct object type (extendedKalmanFilter). Note:
            % the saveobj is inherited.
            
            % Assign non-dependent public properties
            retEKF = extendedKalmanFilter...
                ('DataType', sobj.pDataType, ...
                'HasAdditiveProcessNoise', sobj.HasAdditiveProcessNoise, ... % set pPredictor
                'HasAdditiveMeasurementNoise', sobj.HasAdditiveMeasurementNoise); % set pCorrector
            % Load the remaining properties
            loadobjHelper(retEKF,sobj);
        end
    end
end
