Multiple input to a function under the handler?

I use trackingEKF from Automated Driving Toolbox -> Detection and Tracking -> Tracking and Sensor Fusion

For this, i've drafted state transition function, which accounts for constant speed and vaying angle of velocity vector, if isOri is specified. if not, it's a regular onstant velocity propagation fcn:

function newstate = tranFcn(filter, dt, isOri)
    % a state transition function.
    % used to propagate the state vector with predict method.
    % phi in a standart Kalman Filtering notation
    
    newstate = zeros(size(filter.State));

    if isOri == 1
        % rotate velocity vector and assign values to newstate(4:6)
    else
        newstate(4) = filter.State(4);
        newstate(5) = filter.State(5);
        newstate(6) = filter.State(6);
    end
        
    newstate(1) = filter.State(1) + newstate(4) * dt;
    newstate(2) = filter.State(2) + newstate(5) * dt;
    newstate(3) = filter.State(3) + newstate(6) * dt;
end

I configure a filter object:

% state pattern is just an array of zeros to represent [x y z vx vy vz qx qy qz qw] 
% [qx qy qz qw] are present if isOri == 1. 
kalmFi = trackingEKF...
    (   @(t, ori) tranFcn(t, ori),...
        @measFcn,...
        statePattern,...
            'HasAdditiveMeasurementNoise', true,...
            'MeasurementNoise', measNoiseMx,...
            'HasAdditiveProcessNoise', true,...
            'ProcessNoise',    processNoiseCovarMx...
    );

Then,

[kalmState{i},~] = kalmFi.predict(dt, isOri);

OR (i tried both ways, still get an error)

[kalmState{i},~] = kalmFi.predict(kalmFi, dt, isOri);

Returns an error on that number of arguments won't match:

Error using matlabshared.tracking.internal.ExtendedKalmanFilter/predict (line 599)
The number of optional arguments in the call to predict does not match the number of
additional arguments expected by the StateTransitionFcn. Check that all additional
arguments of StateTransitionFcn are provided as optional input arguments to predict.
Error in trackingEKF/predict (line 157)
           [varargout{1:nargout}] =
           [e-mail](filter,varargin{:});
           Error in fooscript (line 50)
    [kalmState{i},~] = kalmFi.predict(dt, isOri);

I just don't know what to do. I can't just write "predict" (not as a method of the filter) because there are other MATLAB functions that i use in the project that react to such a function call.

Answer 1

[Altaïr]

Hey @Arman Petrosyants,

When the process noise is included in the state vector (i.e., HasAdditiveProcessNoise is set to true), the StateTransitionFcn can use either of the following syntaxes:

x(k) = statetransitionfcn(x(k-1))
% OR
x(k) = statetransitionfcn(x(k-1),parameters)

Here, x(k) represents the state at time k, and parameters refer to any additional arguments needed by the state transition function. When creating the StateTransitionFcn handle for the trackingEKF function, only the state should be included as the argument. Any extra parameters should be incorporated when defining the handle.

Below is a simple example illustrating this approach:

dt = 1; % Step time
initialState = [0; 1; 0; 1]; % Initial state [x; vx; y; vy]

ekf = trackingEKF('StateTransitionFcn', @(x) myStateTransitionFcn(x, dt), ...
                  'MeasurementFcn', @myMeasurementFcn, ...
                  'State', initialState, ...
                  'StateTransitionJacobianFcn', @(x) myStateTransitionJacobianFcn(x, dt), ...
                  'MeasurementJacobianFcn', @myMeasurementJacobianFcn, ...
                  'ProcessNoise', diag([0.1, 0.1, 0.1, 0.1]), ...
                  'MeasurementNoise', diag([0.5, 0.5]));
                  % Simulate a measurement
z = [2; 2]; % Example measurement [x; y]

% Predict the next state
predict(ekf);

% Correct the state with the measurement
correct(ekf, z);

% Display the updated state
disp(ekf.State);

function J = myMeasurementJacobianFcn(x)
    J = [1, 0, 0, 0;
         0, 0, 1, 0];
end
function x_next = myStateTransitionFcn(x, dt)
    % x: current state [x; vx; y; vy]
    % dt: time step
    A = [1, dt, 0, 0;
         0, 1,  0, 0;
         0, 0,  1, dt;
         0, 0,  0, 1];
    x_next = A * x;
end
function J = myStateTransitionJacobianFcn(x, dt)
    J = [1, dt, 0, 0;
         0, 1,  0, 0;
         0, 0,  1, dt;
         0, 0,  0, 1];
end
function z = myMeasurementFcn(x)
    % x: state [x; vx; y; vy]
    z = [x(1); x(3)]; % [x; y]
end

In this example, the myStateTransitionFcn function requires the time step, dt, as an additional argument. When setting up the StateTransitionFcn handle, dt is included as shown below:

@(x) myStateTransitionFcn(x, dt)

For further details on the input arguments available in trackingEKF and thier functions, the following command can be used:

web(fullfile(docroot, 'driving/ref/trackingekf.html'))