%particleFilter Create a particle filter state estimator
%   The particle filter is a recursive, Bayesian state estimator that
%   uses discrete particles to approximate the posterior distribution
%   of the estimated state.
%
%   The particle filter algorithm computes this state estimate recursively
%   and involves two steps, prediction and correction (also known as
%   the update step).
%
%   The prediction step uses the previous state to predict the current
%   state based on a given system model. The correction step uses the
%   current sensor measurement to correct the state estimate. The
%   algorithm periodically redistributes, or resamples, the particles
%   in the state space to match the posterior distribution of the
%   estimated state.
%   The particle filter can be applied to arbitrary non-linear system
%   models and process and measurement noise can follow arbitrary
%   non-Gaussian distributions.
%
%   The estimated state consists of a number of state variables. Each
%   particle represents a discrete state hypothesis. The set of all
%   particles approximates the posterior distribution of the estimated
%   state.
%
%
%   obj = particleFilter() returns a particle filter object with default
%   state transition function and measurement likelihood function. 
%
%   obj = particleFilter(StateTransitionFcn, MeasurementLikelihoodFcn) lets
%   you specify the state transition function, f, and the measurement
%   likelihood model, h. Both must be a function_handle. Use the initialize
%   method to initialize the particles with a known mean and covariance or
%   uniformly distributed within defined state bounds.
%
%   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 Particles 
%   property of the particle filter is 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 Particles and Weights properties of the particle filter is
%   overwritten with the correction results.
%
%   [x_corr, P_corr] = correct(obj, y, varargin) additionally, lets you
%   specify input arguments that will be passed to and used by the
%   MeasurementLikelihoodFcn. For instance, you can use this syntax to pass
%   a known system input, u, to the MeasurementLikelihoodFcn.
%
%   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(myParticleFilter);
%          stateEstimate = correct(myParticleFilter, myMeasurement);
%       Else
%          stateEstimate = predict(myParticleFilter);
%       End
%
%
%   particleFilter methods:
%       initialize       - Initialize the state of the particle filter
%       predict          - Calculate the predicted state in the next time step
%       correct          - Adjust state estimate based on sensor measurement
%       getStateEstimate - Extract the best state estimate and covariance from the particles
%       copy             - Create a copy of the particle filter
%
%   particleFilter properties:
%       NumStateVariables        - (Read-only) Number of state variables for the particle filter
%       NumParticles             - (Read-only) Number of particles used in the filter
%       StateTransitionFcn       - Callback function calculating the state transition
%       MeasurementLikelihoodFcn - Callback function calculating the likelihood of sensor measurements
%       IsStateVariableCircular  - (Read-only) Indicator if state variables have a circular distribution
%       ResamplingPolicy         - Policy settings that determine when to trigger resampling
%       ResamplingMethod         - Method used for particle resampling
%       StateEstimationMethod    - Method used for state estimation
%       Particles                - Array of particle values
%       Weights                  - Vector of particle weights
%       State                    - (Read-only) Current state estimate
%       StateCovariance          - (Read-only) Current state estimation error covariance
%
%
%   Example: 
%
%     % 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);
% 
%     % Create a particle filter
%     % Set state transition and measurement likelihood functions
%     myPF = particleFilter(@vdpParticleFilterStateFcn,@vdpMeasurementLikelihoodFcn);
% 
%     % Initialize the particle filter at state [2; 0] with unit covariance
%     % Use 1000 particles
%     initialize(myPF, 1000, [2;0], eye(2));
% 
%     % Pick the mean state estimation method and systematic resampling
%     myPF.StateEstimationMethod = 'mean';
%     myPF.ResamplingMethod = 'systematic';
% 
%     % Estimate
%     xEst = zeros(size(xTrue));
%     for k=1:size(xTrue,1)
%         % Use measurement y[k] to correct the particles for time k
%         xEst(k,:) = correct(myPF,yMeas(k));
%         % The result is x[k|k]: Estimate of states at time k, utilizing
%         % measurements up to time k. This estimate is the mean of all particles
%         % because StateEstimationMethod was 'mean'.
%         %
%         % Now, predict particles at next time step. These are utilized in the
%         % next correct command
%         predict(myPF); % Filter stores Particles[k+1|k]
%     end
%
%     % Plot results
%     figure(1)
%     plot(xTrue(:,1),xTrue(:,2),'x',xEst(:,1),xEst(:,2),'ro');
%     legend('True','Estimated');
% 
%  See also: extendedKalmanFilter, unscentedKalmanFilter, kalman, kalmd

%   Copyright 2017-2022 The MathWorks, Inc.

%#codegen

classdef particleFilter < matlabshared.tracking.internal.ParticleFilter
    methods
        function PF = particleFilter(varargin)
            controllib.internal.util.initializeFilter();
            PF@matlabshared.tracking.internal.ParticleFilter(varargin{:});
            if nargin==0
                PF.StateTransitionFcn = @vdpParticleFilterStateFcn;
                PF.MeasurementLikelihoodFcn = @vdpMeasurementLikelihoodFcn;
                initialize(PF, 1000, [0;0], eye(2));
            end            
        end
    end
    
    methods (Access = {?matlabshared.tracking.internal.ParticleFilter, ?matlab.unittest.TestCase})
        function resamplePolicy = defaultResamplingPolicy(~)
            %defaultResamplingPolicy The object determining when resampling should occur
            
            resamplePolicy = particleResamplingPolicy();
        end
        
        function stateTransFcn = defaultStateTransitionFcn(~)
            %defaultStateTransitionFcn The default value for the StateTransitionFcn property
            
            stateTransFcn = @vdpParticleFilterStateFcn;
        end
        
        function measLhoodFcn = defaultMeasurementLikelihoodFcn(~)
            %defaultMeasurementLikelihoodFcn The default value for the MeasurementLikelihoodFcn property
            
            measLhoodFcn = @vdpMeasurementLikelihoodFcn;
        end
        
        function stateOrientation = defaultStateOrientation(~)
            %defaultMeasurementLikelihoodFcn The default value for the MeasurementLikelihoodFcn property
            stateOrientation = 'column';
        end
    end
end
