gmphd

Gaussian mixture (GM) PHD filter

Description

The gmphd object is a filter that implements the probability hypothesis density (PHD) using a mixture of Gaussian components. The filter assumes the target states are Gaussian and represents these states using a mixture of Gaussian components. You can use a gmphd filter to track extended objects or point targets. In tracking, a point object returns at most one detention per sensor scan, and an extended object can return multiple detections per sensor scan.

You can directly create a gmphd filter. You can also initialize a gmphd filter used with trackerPHD by specifying the FilterInitializationFcn property of trackingSensorConfiguration. You can use the provided initcvgmphd, initctgmphd, initcagmphd, and initctrectgmphd as initialization functions. Or, you can create your own initialization functions.

Creation

Syntax

phd = gmphd

phd = gmphd(states,stateCovariances)

phd = gmphd(states,stateCovariances,Name,Value)

Description

phd = gmphd creates a gmphd filter with default property values.

phd = gmphd(states,stateCovariances) allows you to specify the states and corresponding state covariances of the Gaussian distribution for each component in the density. states and stateCovariances set the States and StateCovariances properties of the filter.

phd = gmphd(states,stateCovariances,Name,Value) also allows you to specify properties for the filter using one or more name-value pairs. Enclose each property name in quotes.

Properties

States — State of each component in filter

P-by-N matrix

State of each component in the filter, specified as a P-by-N matrix, where P is the dimension of the state and N is the number of components. Each column of the matrix corresponds to the state of one component. The default value for States is a 6-by-2 matrix, in which the elements of the first column are all 0, and the elements of the second column are all 1.

Data Types: single | double

StateCovariances — State estimate error covariance of each component in filter

P-by-P-by-N array

State estimate error covariance of each component in the filter, specified as a P-by-P-by-N array, where P is the dimension of the state and N is the number of components. Each page (P-by-P matrix) of the array corresponds to the covariance matrix of each component. The default value for StateCovariances is a 6-by-6-by-2 array, in which each page (6-by-6 matrix) is an identity matrix.

Data Types: single | double

StateTransitionFcn — State transition function

@constvel (default) | function handle

State transition function, specified as a function handle. This function calculates the state vector at time step k from the state vector at time step k–1. The function can also include noise values.

• If HasAdditiveProcessNoise is true, specify the function using one of these syntaxes:

  x(k) = transitionfcn(x(k-1))
  

  x(k) = transitionfcn(x(k-1),dT)

  where x(k) is the state estimate at time k, and dT is the time step.

• If HasAdditiveProcessNoise is false, specify the function using one of these syntaxes:

  x(k) = transitionfcn(x(k-1),w(k-1))
  

  x(k) = transitionfcn(x(k-1),w(k-1),dT)

  where x(k) is the state estimate at time k, w(k) is the process noise at time k, and dT is the time step.

  For more details on the state transition model, see Algorithms for trackingEKF.

Example: @constacc

Data Types: function_handle

StateTransitionJacobianFcn — Jacobian of state transition function

@constveljac (default) | function handle

Jacobian of the state transition function, specified as a function handle. This function has the same input arguments as the state transition function.

• If HasAdditiveProcessNoise is true, specify the Jacobian function using one of these syntaxes:

  Jx(k) = statejacobianfcn(x(k))
  

  Jx(k) = statejacobianfcn(x(k),dT)

  where x(k) is the state at time k, dT is the time step, and Jx(k) denotes the Jacobian of the state transition function with respect to the state. The Jacobian is a P-by-P matrix at time k, where P is the dimension of the state.

• If HasAdditiveProcessNoise is false, specify the Jacobian function using one of these syntaxes:

  [Jx(k),Jw(k)] = statejacobianfcn(x(k),w(k))
  

  [Jx(k),Jw(k)] = statejacobianfcn(x(k),w(k),dT)

  where w(k) is a Q-element vector of the process noise at time k. Unlike the case of additive process noise, the process noise vector in the non-additive noise case doesn't need to have the same dimensions as the state vector.

  Jw(k) denotes the P-by-Q Jacobian of the predicted state with respect to the process noise elements, where P is the dimension of the state.

If not specified, the Jacobians are computed by numerical differencing at each call of the predict function. This computation can increase the processing time and numerical inaccuracy.

Example: @constaccjac

Data Types: function_handle

ProcessNoise — Process noise covariance

eye(3) (default) | positive real-valued scalar | positive definite real-valued matrix

Process noise covariance:

• When HasAdditiveProcessNoise is true, specify the process noise covariance as a real-valued scalar or a positive definite P-by-P matrix. P is the dimension of the state vector. When specified as a scalar, the matrix is a multiple of the P-by-P identity matrix.

• When HasAdditiveProcessNoise is false, specify the process noise covariance as a Q-by-Q matrix. Q is the size of the process noise vector. You must specify ProcessNoise before any call to the predict object function.

Example: [1.0 0.05; 0.05 2]

HasAdditiveProcessNoise — Model additive process noise

false (default) | true

Option to model process noise as additive, specified as true or false. When this property is true, process noise is added to the state vector. Otherwise, noise is incorporated into the state transition function.

Example: true

HasExtent — Indicate if components have extent

false (default) | true

Indicate if components have extent, specified as true or false. Set this property to true if the filter is intended to track extended objects. An extended object can generate more than one measurement per sensor scan. Set this property to false if the filter is only intended to track point targets.

Example: true

MeasurementOrigin — Origination of measurements from extended objects

'center' (default) | 'extent'

Origination of measurements from extended objects, specified as:

• 'center' — The filter assumes the measurements originate from the mean state of a target. This approach is applicable when the state does not model the extent of the target even though the target may generate more than one measurement.

• 'extent' — The filter assumes measurements are not centered at the mean state of a target. For computational efficiency, the expected measurement is often calculated as a function of the reported measurements specified by the measurement model function.

Note that the function setups of MeasurementFcn and MeasurementJacobianFcn are different for 'center' and 'extent' options. See the descriptions of MeasurementFcn and MeasurementJacobianFcn for more details.

Dependencies

To enable this property, set the HasExtent property to 'true'.

Data Types: double

Labels — Label of each component in mixture

[0 0] (default) | 1-by-N row vector of nonnegative integer

Label of each component in the mixture, specified as a 1-by-N row vector of nonnegative integers. N is the number of components in the mixture. Each component can only have one label, but multiple components can share the same label.

Example: [1 2 3]

Data Types: single | double

Weights — Weight of each component in mixture

[1 1] (default) | 1-by-N row vector of positive real value

Weight of each component in the mixture, specified as a 1-by-N row vector of positive real values. N is the number of components in the mixture. The weight of each component is given in the same order as the Labels property.

Example: [1.1 0.82 1.1]

Data Types: single | double

Detections — Detections

D-element cell array of objectDetection objects

Detections, specified as a D-element cell array of objectDetection objects. You can create detections directly, or you can obtain detections from the outputs of sensor objects, such as radarSensor, monostaticRadarSensor, irSensor, and sonarSensor.

Data Types: single | double

MeasurementFcn — Measurement model function

@cvmeas (default) | function handle

Measurement model function, specified as a function handle. This function specifies the transition from state to measurement. Depending on the HasExtent and MeasurementOrigin properties, the measurement model function needs to be specified differently:

1. HasExtent is false, or HasExtent is true and MeasurementOrigin is 'center'. In these two cases,

   • If HasAdditiveMeasurementNoise is true, specify the function using one of these syntaxes:

     z = measurementfcn(x)
     

     z = measurementfcn(x,parameters)

     where the P-by-N matrix x is the estimated Gaussian states at time k and x(:,i) represents the ith state component in the mixture. The M-by-N matrix z is the corresponding measurement, and z(:,i) represents the measurement resulting from the ith component. Parameters are MeasurementParameters provided in the objectDetections set in the Detections property.

   • If HasAdditiveMeasurementNoise is false, specify the function using one of these syntaxes:

     z = measurementfcn(x,v)
     

     z = measurementfcn(x,v,parameters)

     where v is an R-dimensional measurement noise vector.

2. HasExtent is true and MeasurementOrigin is 'extent'. In this case, the expected measurements originate from the extent of the target and rely on the actual distribution of the detections:

   • If HasAdditiveMeasurementNoise is true, specify the function using:

     z = measurementfcn(x,detections)
     

     where the P-by-N matrix x is the estimated Gaussian states at time k and x(:,i) represents the ith state component in the mixture. detections is a cell array of objectDetection objects, and z is the expected measurement. Note that z(:,i,j) must return the expected measurement based on the ith state component and the jth objectDetection in detections.

   • If HasAdditiveMeasurementNoise is false, specify the function using:

     z = measurementfcn(x,v,detections)
     

     where v is an R-dimensional measurement noise vector.

HasExtent	MeasurementOrigin	Measurement Function	Note
false	NA	HasAdditiveMeasurementNoise Syntaxes true z = measurementfcn(x) z = measurementfcn (x,para) false z = measurementfcn(x,v) z = measurementfcn (x,v,para)	x(:,i) represents the ith state component in the mixture. z(:,i) represents the measurement resulting from the ith component.
true	'center'
true	'extent'	HasAdditiveMeasurementNoise Syntaxes true z = measurementfcn(x,detections) false z = measurementfcn(x,v,detections)	x(:,i) represents the ith state component in the mixture. z(:,i,j) must return the expected measurement based on the ith state component and the jth objectDetection in detections.

Data Types: function_handle

MeasurementJacobianFcn — Jacobian of measurement function

@cvmeasjac (default) | function handle

Jacobian of the measurement function, specified as a function handle. Depending on the HasExtent and MeasurementOrigin properties, the measurement Jacobian function needs to be specified differently:

1. HasExtent is false, or HasExtent is true and MeasurementOrigin is 'center'. In these two cases:

   • If HasAdditiveMeasurmentNoise is true, specify the Jacobian function using one of these syntaxes:

     Jmx = measjacobianfcn(x)

     Jmx = measjacobianfcn(x,parameters)

     where the P-element vector x is one state component at time k and Jmx is the M-by-P Jacobian of the measurement function with respect to the state. M is the dimension of the measurement. Parameters are MeasurementParameters provided in the objectDetections set in the Detections property.

   • If HasAdditiveMeasurmentNoise is false, specify the Jacobian function using one of these syntaxes:

     [Jmx,Jmv] = measjacobianfcn(x,v)
     

     [Jmx,Jmv] = measjacobianfcn(x,v,parameters)

     where v is an R-dimensional measurement noise vector, and Jmv is the M-by-R Jacobian of the measurement function with respect to the measurement noise.

2. HasExtent is true and MeasurementOrigin is 'extent'. In this case, the expected measurements originate from the extent of the target and rely on the actual distribution of the detections. The measurement Jacobian function must support one of these two syntaxes:

   • If HasAdditiveMeasurmentNoise is true, specify the Jacobian function using:

     Jmx = measjacobianfcn(x,detections)
     

     where x is one state estimate component at time k. detections is a set of detections defined as a cell array of objectDetection objects. Jmx denotes the M-by-P-by-D Jacobian of the measurement function with respect to the state. M is the dimension of the measurement, P is the dimension of the state, and D is the number of objectDetection objects in detections.

   • If HasAdditiveMeasurmentNoise is false, specify the Jacobian function using:

     [Jmx,Jmv] = measjacobianfcn(x,v,detections)
     

     where v is an R-dimensional measurement noise vector, and Jmv is the M-by-R-by-D Jacobian of the measurement function with respect to the measurement noise.

   Note that Jmx(:,:,j) must define the state Jacobian corresponding to the jth objectDetection in detections. Jmv(:,:,j) defines the measurement noise Jacobian corresponding to the jth objectDetection in detections.

HasExtent	MeasurementOrigin	Measurement Jacobian Function	Note
false	NA	HasAdditiveMeasurementNoise Syntaxes true Jmx = measjacobianfcn(x) Jmx = measjacobianfcn(x,para) false [Jmx,Jmv] = measjacobianfcn(x,v) [Jmx,Jmv] = measjacobianfcn(x,v,para)	x is only one Gaussian component in the mixture.
true	'center'
true	'extent'	HasAdditiveMeasurementNoise Syntaxes true z = measurementfcn(x,detections) false z = measurementfcn(x,v,detections)	Jmx(:,:,j) defines the state Jacobian corresponding to the jth objectDetection in detections. Jmv(:,:,j) defines the measurement noise Jacobian corresponding to the jth objectDetection in detections.

Data Types: function_handle

HasAdditiveMeasurementNoise — Model additive measurement noise

false (default) | true

Option to model measurement noise as additive, specified as true or false. When this property is true, measurement noise is added to the state vector. Otherwise, noise is incorporated into the measurement function.

Example: true

MaxNumDetections — Maximum number of detections

1000 (default) | positive integer

Maximum number of detections the gmphd filter can take as input, specified as a positive integer.

Example: 50

Data Types: single | double

MaxNumComponents — Maximum number of components

1000 (default) | positive integer

Maximum number of components the gmphd filter can maintain, specified as a positive integer.

Data Types: single | double

Object Functions

predict	Predict probability hypothesis density of phd filter
correctUndetected	Correct phd filter with no detection hypothesis
correct	Correct phd filter with detections
likelihood	Log-likelihood of association between detection cells and components in the density
append	Append two phd filter objects
merge	Merge components in the density of phd filter
scale	Scale weights of components in the density
prune	Prune the filter by removing selected components
labeledDensity	Keep components with a given label ID
extractState	Extract target state estimates from the phd filter
clone	Create duplicate phd filter object

Examples

Run gmphd Filter for Point Objects

Create a filter with two 3-D constant velocity components. The initial state of one component is [0;0;0;0;0;0]. The initial state of the other component is [1;0;1;0;1;0]. Each component is initialized with position covariance equal to 1 and velocity covariance equal to 100.

states = [zeros(6,1) [1;0;1;0;1;0]];
cov1 = diag([1 100 1 100 1 100]);
covariances = cat(3,cov1,cov1);
phd = gmphd(states, covariances, 'StateTransitionFcn', @constvel,...
    'StateTransitionJacobianFcn',@constveljac,...
    'MeasurementFcn',@cvmeas,...
    'MeasurementJacobianFcn',@cvmeasjac,...
    'ProcessNoise', eye(3),...
    'HasAdditiveProcessNoise',false);
    

Predict the filter 0.1 time step ahead.

predict(phd,0.1);

Define three detections using ojbectDetection.

rng(2019);
detections = cell(3,1);
detections{1} = objectDetection(0,[1;1;1] + randn(3,1));
detections{2} = objectDetection(0,[0;0;0] + randn(3,1));
detections{3} = objectDetection(0,[4;5;5] + randn(3,1));
phd.Detections = detections;

Calculate the likelihood of each detection. For a point-target filter, the partition of detections is unnecessary, and each detection occupies a cell. Therefore, detectionIndices is an identity matrix. The resulting likelihood of detection 1 and 2 is higher than that of detection 3 because they are closer to the components.

detectionIndices = logical(eye(3)); 
logLikelihood = likelihood(phd,detectionIndices)

logLikelihood = 2×3

   -5.2485   -4.7774  -22.8899
   -4.5171   -5.0008  -17.3973

Correct the filter with the scaled likelihood.

lhood = exp(logLikelihood);
lhood = lhood./sum(lhood,2);
correct(phd,detectionIndices,lhood); 

Merge the components with a merging threshold equal to 1.

merge(phd,1);

Extract state estimates with an extract threshold equal to 0.5.

minWeight = 0.5;
targetStates = extractState(phd,minWeight);
[ts1,ts2]= targetStates.State;

Visualize the results.

% Extract the measurements.
d = [detections{:}];
measurements = [d.Measurement];
% Plot the measurements and estimates.
figure()
plot3(measurements(1,:),measurements(2,:),measurements(3,:),'x','MarkerSize',10,'MarkerEdgeColor','b');
hold on;
plot3(ts1(1),ts1(3),ts1(5),'ro');
hold on;
plot3(ts2(1),ts2(3),ts2(5),'ro');
xlabel('x')
ylabel('y')
zlabel('z')
hold on;
legend('Detections','Components')

References

[1] Vo, B. -T, and W. K. Ma. "The Gaussian mixture Probability Hypothesis Density Filter." IEEE Transactions on Signal Processing, Vol, 54, No, 11, pp. 4091–4104, 2006.

[2] Granstrom, Karl, Christian Lundquist, and Omut Orguner. "Extended target tracking using a Gaussian-mixture PHD filter." IEEE Transactions on Aerospace and Electronic Systems 48, no. 4 (2012): 3268-3286.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

• The code generation configuration must allow recursion to use the merge method.