Variable measurement length for trackingEKF

I have the following issue where a protected property (pN) inside the trackingEKF is set when using the "distance" function.

I have a scenario where available measurements vary and therefore I set the measurement function and noise accordingly. The issue comes from a protected property that validates if my "correct" step have the right dimensions.

E.g.

I initialize with 4 measurements and associate with the "distance" function. pN is then set to 4. If the association is correct I want to include these.

In the next step I change the EKF property MeasurementFcn and MeasurementNoise to a dimension of 7. This does not update the protected "pN" propery inside ExtendedKalmanFilter.

In the correction step I get the following error:

"Error using coder.internal.assert (line 33)

Expected z_matrix to be a vector of 4 elements or a matrix of 4 columns."

Any suggestions on how to bypass this without initializing a new filter?

Answer 1

[Elad Kivelevitch]

Hi,

Due to the need to support code generation, the sizes of state, state covariance, process noise, and measurement noise all have to be fixed in the filter.

To be able to support your use case, you can modify your definition of measurement function and measurement noise in the following way:

For the measurement function, define a function that allows measurement parameters to be passed into the function. As one of those measurement parameters pass an ID of the sensor from which the measurement was obtained. For example:

function z = myMeas(x,sensorID,param1,param2)
    z = zeros(7,1,'like',x); % The largest measurement size, assuming the class of x is the same as measurement
    if sensorID == 1
        z(1:4) = firstModel(x,param1,param2); % That's the four-element measurement model
    else   
        z(:) = secondModel(x,param1,param2);  % That's the seven-element measurement model
    end
end

Similarly, you can create a myMeasJacobian function

Now, when you get measurements from your sensors, before you actually pass them to the filter, you will have to pre-process them to the largest measurement length. For example:

function paddedMeas = padMeasurement(originalMeas,sensorID)
paddedMeas = zeros(7,1,'like',originalMeas);
if sensorID == 1
    paddedMeas(1:4) = originalMeas(:);
else
    paddedMeas(:) = originalMeas(:);
end
end

A similar function can be written for the measurement noise.

HTH,

Elad

[Riccardo Bonetti]

I'm tring to implement what you described but a doubt arises to me: how do I differentiate the padded zeros with a measurement whose real value is zero? I mean, my measurements are position measurements over a grid, how does this algorithm know that the added 0 is not a measurement but a padded 0?

Hope my question is clear. Thank you in advance.

[Elad Kivelevitch]

The tracker uses your measurement function to do that.

As I described in the above reply, if you specify the sensor index as part of your measurement parameters, it will be sent to your specified function and then based on sensorID it will select the right elements in the measurement vector.

Answer 2

[Honglei Chen]

Could you elaborate what kind of system you are trying to model? In general the dimension of the measurement doens't change over time but it looks like you have a special use case you want to address? Thanks.

[Albin Westin]

Thank you for your answer.

I have a multi sensor configuration in a multi vehicle tracking scenario. The sensors give uncorrelated measurements and are not associated to the same track. Hence, I want to associate the active tracks in frame with the available measurement (varying).

For example, if I only get measurement from sensor 1 in frame k: do measurement update with and . Where .

if I get measurement from sensor 2 in frame k: do measurement update with and . Where .

And the last case with measurement from both sensor 1 and 2: , and.

The association is done in two steps, first associating with the toolbox function distance() and then the second measurement from sensor 2 with once again distance(). This is the first approach I tried to handle varying available measurements (or multiple sensors in general) and seems to be a common way to solve this problem.