How do I use an Extended Kalman Filter instead of a Kalman Filter in the "Sensor Fusion Using Synthetic Radar and Vision Data" example

I am using the following example:

https://www.mathworks.com/help/driving/ug/sensor-fusion-using-synthetic-radar-and-vision-data.html

Here, instead of using the "trackingKF" function, I want to use the "trackingEKF" function, so I replace the following code:

filter = trackingKF('MotionModel', '2D Constant Velocity', 'State', H' * detection.Measurement, 'MeasurementModel', H, 'StateCovariance', H' * detection.MeasurementNoise * H, 'MeasurementNoise', detection.MeasurementNoise); 

with

filter = trackingEKF(@constvel, @cvmeas, H' * detection.Measurement, 'StateTransitionJacobianFcn', @constveljac, 'MeasurementJacobianFcn', @cvmeasjac); 

When I do that, I receive the following error messages: 

ERROR: Error using coder.internal.assert (line 33)
The output from MeasurementFcn must have the same size and data type as the given measurement.
...
...

I want to know what changes should I make to use trackingEKF in this example.

Answer 1

[MathWorks Support Team]

The state you are trying to use is incompatible with the models you are using for state transition (constvel) and measurement (cvmeas). Both these models expect to have a 3-D constant velocity state [x;vx;y;vy;z;vz] and cvmeas provides a measurement that is 3-D position measurement [x;y;z], with the default MeasurementParameters value. 

If you want to use the "trackingEKF" function with the models as described above, you will have to write your own state transition function, measurement function, and corresponding Jacobian functions. Here are some initial suggestions for the code for these 4 functions:

% State transition function: 
function state = constvel2d(state, dt, varargin)
% Validate that state is 4-elements long and that there are two inputs
A1d = [1 dt; 0 1];
A = blkdiag(A1d,A1d);
state(:) = A*state(:);
end

% Measurement function:
function meas = cvmeas2DFullState(state, varargin)
% Validate that state is 4-elements long
H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1]; 
meas = H*state(:);
end

% State transition Jacobian:
function jacobian = constvel2djac(~, dt, varargin)
% Validate that state is 4-elements long and that there are two inputs
A1d = [1 dt; 0 1];
jacobian = blkdiag(A1d,A1d);
end

% Measurement Jacobian function:
function jacobian = cvmeas2DFullStateJac(state,varargin)
% Validate that state is 4-elements long
jacobian = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1]; 
end

After adding the above code to the "SensorFusionUsingSyntheticDataExample.m" file, the initSimDemoFilter function would look like the following:

function filter = initSimDemoFilter(detection)
H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1];
filter = trackingEKF('State', H' * detection.Measurement, ...
      'StateTransitionFcn', @constvel2d, ...
      'MeasurementFcn', @cvmeas2DFullState, ...
      'StateTransitionJacobianFcn', @constvel2djac, ...
      'MeasurementJacobianFcn', @cvmeas2DFullStateJac); 
end