Function parameter estimation using Particle Filter

After reading the docs about "stateEstimatorPF" I get a little confused about how to create the StateTransitionFcn for my case. In my case I have 10 sensors measurments that decay exponentially and I want to find the best parameters for my function model.

The function model is x = exp(B*deltaT)*x_1, where x are the hypotheses, deltaT is the constant time delta in my measurments and x_1 is the true previous state. I would like to use the particle filter to estimate the parameter B. If I guess right, B should be the particles and the weighted mean of this particles should be what I'm looking for.

How can I write the StateTransitionFcn and use the "stateEstimatorPF" to solve this problem?

The code below is what I get so far (and it does not work):

pf = robotics.ParticleFilter
pf.StateTransitionFcn = @stateTransitionFcn
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';
initialize(pf,5000,[0.9],1);

measu = [1.0, 0.9351, 0.8512, 0.9028, 0.7754, 0.7114, 0.6830, 0.6147, 0.5628, 0.7090]
states = []

for i=1:10
    [statePredicted,stateCov] = predict(pf);
    [stateCorrected,stateCov] = correct(pf,measu(i));
    states(i) = getStateEstimate(pf)
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function predictParticles = stateTransitionFcn(pf, prevParticles,x_1)
    predictParticles = exp(prevParticles)*x_1 %how to properly use x_1?%;
end

Answer 1

[Akshai Manchana]

Hi Alan,

Usually for function parameter estimation problems we use curve fit algorithms. For example look at the following.

% Same measurments as your question

measu = [1.0, 0.9351, 0.8512, 0.9028, 0.7754, 0.7114, 0.6830, 0.6147, 0.5628, 0.7090]

% Expected prediction model

predictionModel = @(B,x) exp(B(1))*x;

% initial guess for function parameter

B0 = 0.9;

% execute least-squares fit

estimatedB =...

lsqcurvefit(model,B0,measu(1:(end-1)),measu(2:end));

% Initial state

x1 = measu(1);

% Use the estimatedB in prediction.

xPred =x1;

for k = 2:length(measu)

xPred = [xPred;model(estimatedB,xPred(k-1))];

end

% Visually observe the difference in actual curve and fitted

plot(measu);

hold on;

plot(xPred);

legend('Actual data', 'Fitted data')

Observe that the fitted curve using above algorithm and actual data.

To learn more about non-linear curve fitting refer to Nonlinear Curve Fitting with lsqcurvefit.

However you can formulate particle filter problem in the following way to estimate function parameters. Observe a few important modifications made to your code.

• Made the function parameter (B*dt) a part of the state to be able to estimate it.
• Since B*dt is expected to be constant, updated state transition function to return previous parameter as is and only predict other part of the state.
• Customized measurement likelihood function to only use part of the state that is not a function parameter.

pf = robotics.ParticleFilter;

pf.StateTransitionFcn = @stateTransitionFcn;

pf.MeasurementLikelihoodFcn = @measurementLikelihoodFcn;

pf.StateEstimationMethod = 'mean';

pf.ResamplingMethod = 'systematic';

measu = [1.0, 0.9351, 0.8512, 0.9028, 0.7754, 0.7114, 0.6830, 0.6147, 0.5628, 0.7090];

% use function parameter as part of state to estimate it

prevState = [measu(1),0.9];

initialize(pf,5000,prevState,eye(2));

states = [measu(1),0.9];

for i=1:9

[statePredicted,stateCov] = predict(pf, measu(i));

[stateCorrected,stateCov] = correct(pf,measu(i+1));

prevState = getStateEstimate(pf);

% all state predictions

states = [states;prevState];

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function predictParticles = stateTransitionFcn(pf, prevParticles,x_1)

% predict using current state

p = exp(prevParticles(:,2)).*x_1;

% predicted function parameter equivalent to previous parameter and only predct function value.

predictParticles = [p,prevParticles(:,2)];

end

% since we are only interested in prediction error of actual states and not

% the complete extended state, customize it.

function likelihood = measurementLikelihoodFcn(pf, predictParticles, measurement)

% The measurement contains all state variables

predictMeasurement = predictParticles;

% Calculate observed error between predicted and actual measurement

% NOTE in this example, we don't have full state observation, but only

% the measurement of current pose, therefore the measurementErrorNorm

% is only based on the pose error.

measurementError = bsxfun(@minus, predictMeasurement(:,1), measurement);

measurementErrorNorm = sqrt(sum(measurementError.^2, 2));

% Normal-distributed noise of measurement

% Assuming measurements on all three pose components have the same error distribution

measurementNoise = 1;

% Convert error norms into likelihood measure.

% Evaluate the PDF of the multivariate normal distribution

likelihood = 1/sqrt((2*pi).^3 * det(measurementNoise)) * exp(-0.5 * measurementErrorNorm);

end