How to define bespoke MeasurementModel with trackingKF
4 views (last 30 days)
William Campbell on 2 Apr 2021
Hello, I am fusing radar and mono-camera data together in a Kalman Filter as part of a tracking algorithm and I am using the sensor input dataset from the MatLab demo “Forward Collision Warning Using Sensor Fusion” which is in a cartesian coordinate frame.
In an extended KF when I use 'trackingEKF' I can call up a bespoke measurement function which switches depending on whether the sensor ID is radar or camera since the filter options accept function handles (refer below showing the 'trackingEKF' calling the bespoke measurement function 'cvmeas2FFullState'), and it runs fine.
filter = trackingEKF('State', H1 * ste, ...
'StateCovariance', steCov, ...
'MeasurementNoise', detectionClusters.MeasurementNoise(1:4,1:4), ...
'StateTransitionFcn', @constvel2d, ...
'MeasurementFcn', @cvmeas2DFullState, ...
'StateTransitionJacobianFcn', @constvel2djac, ...
% Measurement function:
function meas = cvmeas2DFullState(state, sensorID, varargin)
% The measurements depend on the sensor type, which is reported by
% the MeasurementParameters property of the objectDetection. The following
% two sensorID values are used:
% sensorID=1: video objects, the measurement is [x;vx;y] i.e. vy not measured.
% sensorID=2: radar objects, the measurement is [x;vx;y;vy].
% The state depends on the motion model used (in this case constant vel):
% Constant velocity state = [x;vx;y;vy]
case 1 % vision data where vy=0
H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 0];
meas = H*state(:);
case 2 % radar data
H = [1 0 0 0; 0 0 1 0; 0 1 0 0; 0 0 0 1];
meas = H*state(:);
However, if I use a linear Kalman Filter 'trackingKF' with the filter option 'MotionModel', 'Custom', then the 'MeasurementModel' doesn't accept function handles and I am not able to call the same bespoke measurement function with the sensor-dependent switching - is there any way I can generate the switching measurement model as in the 'trackingEKF' ?
Also, if I use an extended Kalman Filter (trackerEKF) with linear dynamic model and linear sensor model, both in cartesian coordinate frame (as in MatLab demo “Forward Collision Warning Using Sensor Fusion”), where the raw radar and camera data is provided in cartesian frame, would this be expected to perform as a linear Kalman Filter ?
Elad Kivelevitch on 20 May 2021
The linear Kalman filter, trackingKF, only uses a single measurement matrix, therefore the way to use two different measurement models is to manually modify them yourself before every call to correct. In other words:
% First set the measurement model based on sensor ID:
KF.MeasurementModel = [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 0];
KF.MeasurementModel = [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1];
% Then, call correct from the right sensor model
Note: this is assuming you just want to use the filter directly. If you want to use the filter inside a tracker, that would be impossible, because the tracker acts on all the detections at the same time and you have no access midway in the step to modify the filter properties.
When you use the trackingEKF with linear functions, you will get similar results as trackingKF with a 'Custom' motion model, assuming that the dt you use in the StateTransitionModel is the same as the dt you will use in the predict call to trackingEKF. That is because the custom motion model in KF assumes a time-invariant state transition model (and process noise).
Of course, if you are using the trackingKF directly (not within a tracker), you can change the StateTransitionModel before each call to predict(KF). It will look like this:
% T is the timestep for the predict, which is known to you.
% StateTransitionModel is a 2-D constant velocity convention: [x;vx;y;vy].
KF.StateTransitionModel = [1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];