For trackingPF in Sensor Fusion and Tracking Toolkit, how to use custom measurement functions returned as a function handle?

My goal is to specify a custom measurement function in the tracking particle filter (trackingPF) from the Sensor Fusion and Tracking Toolkit. I can do it by direct reference to a custom function, but I'm getting an error when trying to do the same with a function handle returned from another function.

Here is the direct approach that works.

%#ok<*NASGU>
priorState = zeros(4,1); % position_x, velocity_x, position_y, velocity_y

% Specify a particle filter with a customMeasurementFcn specified directly.
pf_direct = trackingPF(@constvel, @customMeasurementFcn, priorState)
prediction = predict(pf_direct) 
posterior = correct(pf_direct, 45)

Fine so far. Now attempt the same with a function handle returned from another function.

customMeasurementFcn_returnedHandle = generateMeasurementFunction
pf_handle = trackingPF(@constvel, @customMeasurementFcn_returnedHandle, priorState)
prediction = predict(pf_handle)
posterior = correct(pf_handle, 45)

The correct() call fails with...

% Error using nargin
% Function customMeasurementFcn_returnedHandle does not exist.
% 
% Error in trackingPF/validateMeasurementFcn (line 1114)
%             narginActual = nargin(PF.MeasurementFcn);
% 
% Error in trackingPF/validateMeasurementRelatedProperties (line 726)
%             validateMeasurementFcn(PF,z,n,funcName,varargin{:});
% 
% Error in trackingPF/correct (line 309)
%             validateMeasurementRelatedProperties(PF,z,'correct',varargin{:});

In contradiction with the error, the measurement function handle in the filter looks right...

pf_handle.MeasurementFcn
% ans = 
%     @customMeasurementFcn_returnedHandle

And I can use the handle on its own...

customMeasurementFcn_returnedHandle(pf_handle.Particles(:,1:10))
% ans = 1×10    
    %     1.8973 -135.1443   62.1872  118.7292 -127.2712  164.5253  -56.4507  131.4280   40.8487  -52.8114

But the same call using the handle stored within the particle filter object fails.

pf_handle.MeasurementFcn(pf_handle.Particles(:,10))
    % Unrecognized function or variable 'customMeasurementFcn_returnedHandle'.

Here are the function definitions.

function predictedParticles = customMeasurementFcn(particles, varargin)
    fullMeasurement = cvmeas(particles, 'spherical'); % azimuth, elevation, range, range rate
    predictedParticles = fullMeasurement(1,:) + randn(1, size(particles,2)); % azimuth plus noise
end
    
function generatedMeasurementFunction = generateMeasurementFunction()
    % Other function customization will happen here.
    generatedMeasurementFunction = @customMeasurementFcn;
end    

Could you please show me how to use a function handle returned from generateMeasurementFunction() as the MeasurementFcn in the trackingPF?

Answer 1

[Prashant Arora]

Hi Mark,

It seems like customMeasurementFcn_returnedHandle is already a function handle.

"@customMeasurementFcn_returnedHandle" declares a handle to function named customMeasurementFcn_returnedHandle. As this function doesn't exist, you get an error. You should use the variable customMeasurementFcn_returnedHandle itself as it is already the handle to your desired function.

You should define your trackingPF as (No @ in customMeasurmentFcn_returnedHandle)

pf_handle = trackingPF(@constvel, customMeasurementFcn_returnedHandle, priorState)

Hope this helps.

Prashant

[Mark Frymire]

That did it, thank you!