Forward Collision Warning Using Sensor Fusion
本例程展示基於融合視覺和雷達傳感器的物體檢測的FCW實現方式。
概述
FCW是駕駛輔助和自動駕駛系統的重要功能，他在即將發生碰撞之前給駕駛員提供精確、及時和可靠的報警信息。爲了實現此功能，汽車裝配有前視視覺和雷達傳感器。爲了提高精確報警的可靠性和最大化的降低錯誤報警的可能性，需要對傳感器信息進行融合。
本例程中，測試車輛裝配有不同類型的傳感器，各傳感器的輸出均會被記錄。本例程中使用的傳感器有：
1. 視覺傳感器， 提供所觀測物體的列表、分類以及車道線信息，物體列表每秒更新10次，車道線每秒更新20次。
2. 中距和長距毫米波雷達，提供未分類的物體列表。物體列表每秒更新20次。
3. IMU，提供車速和轉向信息，每秒更新20次。
4. 攝像機，錄製車輛前方場景的視頻。備註：算法不使用視頻信息，只爲顯示追蹤結果以作算法驗證。
FCW功能實現由以下步驟組成：
1. 傳感器信息採集；
2. 融合傳感器信息，生成跟蹤目標列表，i.e. 車輛前方物體的估計位置和速度；
3. 基於跟蹤目標和FCW策略的報警。FCW策略基於Euro NCAP AEB測試流程，並同時參考車輛前方物體的相對距離和相對速度；
更多詳細關於多目標跟蹤的信息，請參考Multiple Object tracking
本例中的顯示信息均基於monoCamera和birdsEyePlot。爲簡潔起見，創建和更新顯示信息被移植入幫助函數中。更多關於顯示信息的使用方法，請參看Annotate Video using Dectections in vehicle coordinates和visualize sensor coverage, detection and tracks.
本例是一個腳本，這裏顯示的主體和文後以local functions形式出現的幫助函數。
% Set up the display
[videoReader, videoDisplayHandle, bepPlotters, sensor] = helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4', 'SensorConfigurationData.mat');
% Read the recorded detections file
[visionObjects, radarObjects, inertialMeasurementUnit, laneReports, ...
timeStep, numSteps] = readSensorRecordingsFile('01_city_c2s_fcw_10s_sensor.mat');
% An initial ego lane is calculated. If the recorded lane information is
% invalid, define the lane boundaries as straight lines half a lane
% distance on each side of the car
laneWidth = 3.6; % meters
egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]);
% Prepare some time variables
time = 0; % Time since the beginning of the recording
currentStep = 0; % Current timestep
snapTime = 9.3; % The time to capture a snapshot of the display
% Initialize the tracker
[tracker, positionSelector, velocitySelector] = setupTracker();
while currentStep < numSteps && ishghandle(videoDisplayHandle)
% Update scenario counters
currentStep = currentStep + 1;
time = time + timeStep;
% Process the sensor detections as objectDetection inputs to the tracker
[detections, laneBoundaries, egoLane] = processDetections(...
visionObjects(currentStep), radarObjects(currentStep), ...
inertialMeasurementUnit(currentStep), laneReports(currentStep), ...
egoLane, time);
% Using the list of objectDetections, return the tracks, updated to time
confirmedTracks = updateTracks(tracker, detections, time);
% Find the most important object and calculate the forward collision
% warning
mostImportantObject = findMostImportantObject(confirmedTracks, egoLane, positionSelector, velocitySelector);
% Update video and birds-eye plot displays
frame = readFrame(videoReader); % Read video frame
helperUpdateFCWDemoDisplay(frame, videoDisplayHandle, bepPlotters, ...
laneBoundaries, sensor, confirmedTracks, mostImportantObject, positionSelector, ...
velocitySelector, visionObjects(currentStep), radarObjects(currentStep));
% Capture a snapshot
if time >= snapTime && time < snapTime + timeStep
snapnow;
end
end
創建多目標跟蹤器
多目標跟蹤器跟蹤視覺和雷達傳感器返回的物體列表中物體，通過融合兩種傳感器的信息，誤報的概率將大大被降低。
setupTracker函數返回多目標跟蹤器。創建多目標跟蹤器時應考慮以下信息：
- FilterInitializationFcn：likely運動和測量模型。這種情況下，物體定義爲勻加速運動。儘管可以通過配置線性卡爾曼濾波實現此模型；initConstantAccelerationFilter卻配置了擴展卡爾曼濾波。參考“卡爾曼濾波定義”部分；
- Assignmentthreshold：檢測距離多遠時會放棄追蹤。默認值是30，如果有檢測到應該被追蹤的物體卻沒有被追蹤，增大該值。如果被追蹤到的物體太遠，減小該值。本例中該值爲35.
- NumCoastingUpdates：追蹤請求被清除之前的滑移次數。術語滑移是指在沒有檢測到目標（預測）的情形下更新跟蹤器。默認值爲5。本例中，跟蹤器每秒被調用20次並且有兩個傳感器，所以不需修改默認值。
- ConfirmationParameters：確認追蹤參數。新的追蹤器初始化後檢測均未分配。一些檢測也許是誤報，所以所有追蹤目標均爲“Tentative”。爲確認追蹤目標，目標必須至少在N次追蹤器更新中被檢測到M次。M,N的選擇依賴於目標的可見性，本例中默認設置爲3次更新中檢測到2次。
setupTracker輸出爲：
-
tracker - 針對本例配置的多目標跟蹤器
-
positionSelector- 指定某一狀態向量元素爲位置信息的矩陣： position = positionSelector * State -
velocitySelector - 指定某一狀態向量爲速度信息的矩陣： velocity = velocitySelector * State
function [tracker, positionSelector, velocitySelector] = setupTracker()
tracker = multiObjectTracker(...
'FilterInitializationFcn', @initConstantAccelerationFilter, ...
'AssignmentThreshold', 35, 'ConfirmationParameters', [2 3], ...
'NumCoastingUpdates', 5);
% The State vector is:
% In constant velocity: State = [x;vx;y;vy]
% In constant acceleration: State = [x;vx;ax;y;vy;ay]
% Define which part of the State is the position. For example:
% In constant velocity: [x;y] = [1 0 0 0; 0 0 1 0] * State
% In constant acceleration: [x;y] = [1 0 0 0 0 0; 0 0 0 1 0 0] * State
positionSelector = [1 0 0 0 0 0; 0 0 0 1 0 0];
% Define which part of the State is the velocity. For example:
% In constant velocity: [x;y] = [0 1 0 0; 0 0 0 1] * State
% In constant acceleration: [x;y] = [0 1 0 0 0 0; 0 0 0 0 1 0] * State
velocitySelector = [0 1 0 0 0 0; 0 0 0 0 1 0];
end
Define a Kalman Filter
定義卡爾曼濾波
上文定義的multipleObjectTracker使用此處定義的濾波器初始化函數創建卡爾曼濾波器（線性，擴展或無損）。而後，濾波器將被用於車輛周圍的物體追蹤。
function filter = initConstantAccelerationFilter(detection)
% This function shows how to configure a constant acceleration filter. The
% input is an objectDetection and the output is a tracking filter.
% For clarity, this function shows how to configure a trackingKF,
% trackingEKF, or trackingUKF for constant acceleration.
%
% Steps for creating a filter:
% 1. Define the motion model and state
% 2. Define the process noise
% 3. Define the measurement model
% 4. Initialize the state vector based on the measurement
% 5. Initialize the state covariance based on the measurement noise
% 6. Create the correct filter
% Step 1: Define the motion model and state
% This example uses a constant acceleration model, so:
STF = @constacc; % State-transition function, for EKF and UKF
STFJ = @constaccjac; % State-transition function Jacobian, only for EKF
% The motion model implies that the state is [x;vx;ax;y;vy;ay]
% You can also use constvel and constveljac to set up a constant
% velocity model, constturn and constturnjac to set up a constant turn
% rate model, or write your own models.
% Step 2: Define the process noise
dt = 0.05; % Known timestep size
sigma = 1; % Magnitude of the unknown acceleration change rate
% The process noise along one dimension
Q1d = [dt^4/4, dt^3/2, dt^2/2; dt^3/2, dt^2, dt; dt^2/2, dt, 1] * sigma^2;
Q = blkdiag(Q1d, Q1d); % 2-D process noise
% Step 3: Define the measurement model
MF = @fcwmeas; % Measurement function, for EKF and UKF
MJF = @fcwmeasjac; % Measurement Jacobian function, only for EKF
% Step 4: Initialize a state vector based on the measurement
% The sensors measure [x;vx;y;vy] and the constant acceleration model's
% state is [x;vx;ax;y;vy;ay], so the third and sixth elements of the
% state vector are initialized to zero.
state = [detection.Measurement(1); detection.Measurement(2); 0; detection.Measurement(3); detection.Measurement(4); 0];
% Step 5: Initialize the state covariance based on the measurement
% noise. The parts of the state that are not directly measured are
% assigned a large measurement noise value to account for that.
L = 100; % A large number relative to the measurement noise
stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2), L, detection.MeasurementNoise(3:4,3:4), L);
% Step 6: Create the correct filter.
% Use 'KF' for trackingKF, 'EKF' for trackingEKF, or 'UKF' for trackingUKF
FilterType = 'EKF';
% Creating the filter:
switch FilterType
case 'EKF'
filter = trackingEKF(STF, MF, state,...
'StateCovariance', stateCov, ...
'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...
'StateTransitionJacobianFcn', STFJ, ...
'MeasurementJacobianFcn', MJF, ...
'ProcessNoise', Q ...
);
case 'UKF'
filter = trackingUKF(STF, MF, state, ...
'StateCovariance', stateCov, ...
'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...
'Alpha', 1e-1, ...
'ProcessNoise', Q ...
);
case 'KF' % The ConstantAcceleration model is linear and KF can be used
% Define the measurement model: measurement = H * state
% In this case:
% measurement = [x;vx;y;vy] = H * [x;vx;ax;y;vy;ay]
% So, H = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0]
%
% Note that ProcessNoise is automatically calculated by the
% ConstantAcceleration motion model
H = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0];
filter = trackingKF('MotionModel', '2D Constant Acceleration', ...
'MeasurementModel', H, 'State', state, ...
'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...
'StateCovariance', stateCov);
end
end
處理並格式化檢測
記錄的信息在輸入追蹤器之前需要處理和格式化，步驟如下：
- 清理雷達檢測到的雜波信息。雷達會檢測到很多禁止的物體：包括護欄，隔離帶和交通標誌等，若這類檢測結果輸入到追蹤器中，就會誤追蹤到道路邊緣的固定物體,因此這類檢測需要移除。車輛前方的靜止或者車輛附近移動的物體會被雷達檢測到；
- 格式化檢測結果並輸入追蹤器，ie，objectDetection數組。請參看文末的processVideo和ProcessRadar幫助函數。
function [detections,laneBoundaries, egoLane] = processDetections...
(visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)
% Inputs:
% visionFrame - objects reported by the vision sensor for this time frame
% radarFrame - objects reported by the radar sensor for this time frame
% IMUFrame - inertial measurement unit data for this time frame
% laneFrame - lane reports for this time frame
% egoLane - the estimated ego lane
% time - the time corresponding to the time frame
% Remove clutter radar objects
[laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);
realRadarObjects = findNonClutterRadarObjects(radarFrame.object,...
radarFrame.numObjects, IMUFrame.velocity, laneBoundaries);
% Return an empty list if no objects are reported
% Counting the total number of object
detections = {};
if (visionFrame.numObjects + numel(realRadarObjects)) == 0
return;
end
% Process the remaining radar objects
detections = processRadar(detections, realRadarObjects, time);
% Process video objects
detections = processVideo(detections, visionFrame, time);
end
跟蹤器更新
To update the tracker, call the updateTracks method with the following inputs:
調用updateTracks函數及以下參數更新追蹤器：
-
跟蹤器 - 先前配置的多目標跟蹤器。參看“多目標跟蹤器創建”部分；
-
檢測 - processDetections生成的objectDetection物體列表；
-
時間 - 當前場景時間；
跟蹤器輸出包含跟蹤目標信息的結構體
尋找MIO並實現FCW
MIO定義爲本車道線距本車最近的跟蹤對象，也即是具有最小正x值的對象。爲了減少誤報，MIO只參考確認的跟蹤目標。
當MIO確定，本車和MIO的相對速度需立即計算。FCW取決於相對距離和相對速度，三種FCW情形如下：
- 預警（黃色）：MIO接近本車，但是距離大於FCW距離；FCW距離根據Euro NCAP AEB規範計算。請注意，MIO和本車的相對速度不同，FCW距離也會不同，接近速度越大FCW距離越大；
- 安全（綠色）：本車道線無車（無MIO），MIO遠離本車，或者相對距離恆定；
- 警報（紅色）：MIO靠近車輛且相對距離小於FCW距離；
Euro NCAP AEB測試規範定義的距離計算方法：
式中:
d(FCW)爲FCW距離；
v(rel)爲兩車相對速遞；
a(max)爲最大減速度，定義爲重力加速度的40%；
function mostImportantObject = findMostImportantObject(confirmedTracks,egoLane,positionSelector,velocitySelector)
% Initialize outputs and parameters
MIO = []; % By default, there is no MIO
trackID = []; % By default, there is no trackID associated with an MIO
FCW = 3; % By default, if there is no MIO, then FCW is 'safe'
threatColor = 'green'; % By default, the threat color is green
maxX = 1000; % Far enough forward so that no track is expected to exceed this distance
gAccel = 9.8; % Constant gravity acceleration, in m/s^2
maxDeceleration = 0.4 * gAccel; % Euro NCAP AEB definition
delayTime = 1.2; % Delay time for a driver before starting to break, in seconds
positions = getTrackPositions(confirmedTracks, positionSelector);
velocities = getTrackVelocities(confirmedTracks, velocitySelector);
for i = 1:numel(confirmedTracks)
x = positions(i,1);
y = positions(i,2);
relSpeed = velocities(i,1); % The relative speed between the cars, along the lane
if x < maxX && x > 0 % No point checking otherwise
yleftLane = polyval(egoLane.left, x);
yrightLane = polyval(egoLane.right, x);
if (yrightLane <= y) && (y <= yleftLane)
maxX = x;
trackID = i;
MIO = confirmedTracks(i).TrackID;
if relSpeed < 0 % Relative speed indicates object is getting closer
% Calculate expected braking distance according to
% Euro NCAP AEB Test Protocol
d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / maxDeceleration;
if x <= d % 'warn'
FCW = 1;
threatColor = 'red';
else % 'caution'
FCW = 2;
threatColor = 'yellow';
end
end
end
end
end
mostImportantObject = struct('ObjectID', MIO, 'TrackIndex', trackID, 'Warning', FCW, 'ThreatColor', threatColor);
end
總結
本例展示了裝配有視覺，雷達和IMU傳感器的車輛的FCW系統的創建過程。objectDetection生成傳感器報告傳入multipleObjectTracker跟蹤器，跟蹤器融合各類信息並跟蹤車輛前方的目標。
嘗試使用不同的跟蹤器參數，觀察不同參數對跟蹤質量的影響。嘗試更改跟蹤濾波器爲trackingKF或者trackingUKF，或者更換運動模型，eg. 勻速或者連續轉向。最後，我們也可以定義自己的運動模型。
輔助函數
readSensorRecordingsFile讀取文件記錄的傳感器數據
function [visionObjects, radarObjects, inertialMeasurementUnit, laneReports, ...
timeStep, numSteps] = readSensorRecordingsFile(sensorRecordingFileName)
% Read Sensor Recordings
% The |ReadDetectionsFile| function reads the recorded sensor data file.
% The recorded data is a single structure that is divided into the
% following substructures:
%
% # |inertialMeasurementUnit|, a struct array with fields: timeStamp,
% velocity, and yawRate. Each element of the array corresponds to a
% different timestep.
% # |laneReports|, a struct array with fields: left and right. Each element
% of the array corresponds to a different timestep.
% Both left and right are structures with fields: isValid, confidence,
% boundaryType, offset, headingAngle, and curvature.
% # |radarObjects|, a struct array with fields: timeStamp (see below),
% numObjects (integer) and object (struct). Each element of the array
% corresponds to a different timestep.
% |object| is a struct array, where each element is a separate object,
% with the fields: id, status, position(x;y;z), velocity(vx,vy,vz),
% amplitude, and rangeMode.
% Note: z is always constant and vz=0.
% # |visionObjects|, a struct array with fields: timeStamp (see below),
% numObjects (integer) and object (struct). Each element of the array
% corresponds to a different timestep.
% |object| is a struct array, where each element is a separate object,
% with the fields: id, classification, position (x;y;z),
% velocity(vx;vy;vz), size(dx;dy;dz). Note: z=vy=vz=dx=dz=0
%
% The timeStamp for recorded vision and radar objects is a uint64 variable
% holding microseconds since the Unix epoch. Timestamps are recorded about
% 50 milliseconds apart. There is a complete synchronization between the
% recordings of vision and radar detections, therefore the timestamps are
% not used in further calculations.
A = load(sensorRecordingFileName);
visionObjects = A.vision;
radarObjects = A.radar;
laneReports = A.lane;
inertialMeasurementUnit = A.inertialMeasurementUnit;
timeStep = 0.05; % Data is provided every 50 milliseconds
numSteps = numel(visionObjects); % Number of recorded timesteps
end
processLanes將傳感器檢測到的車道線轉換爲parabolicLaneBoundary車道線並保持連續的本車道線估計
function [laneBoundaries, egoLane] = processLanes(laneReports, egoLane)
% Lane boundaries are updated based on the laneReports from the recordings.
% Since some laneReports contain invalid (isValid = false) reports or
% impossible parameter values (-1e9), these lane reports are ignored and
% the previous lane boundary is used.
leftLane = laneReports.left;
rightLane = laneReports.right;
% Check the validity of the reported left lane
cond = (leftLane.isValid && leftLane.confidence) && ...
~(leftLane.headingAngle == -1e9 || leftLane.curvature == -1e9);
if cond
egoLane.left = cast([leftLane.curvature, leftLane.headingAngle, leftLane.offset], 'double');
end
% Update the left lane boundary parameters or use the previous ones
leftParams = egoLane.left;
leftBoundaries = parabolicLaneBoundary(leftParams);
leftBoundaries.Strength = 1;
% Check the validity of the reported right lane
cond = (rightLane.isValid && rightLane.confidence) && ...
~(rightLane.headingAngle == -1e9 || rightLane.curvature == -1e9);
if cond
egoLane.right = cast([rightLane.curvature, rightLane.headingAngle, rightLane.offset], 'double');
end
% Update the right lane boundary parameters or use the previous ones
rightParams = egoLane.right;
rightBoundaries = parabolicLaneBoundary(rightParams);
rightBoundaries.Strength = 1;
laneBoundaries = [leftBoundaries, rightBoundaries];
end
findNonClutterRadarObjects移除雷達檢測中的被確認爲雜波的信息；
function realRadarObjects = findNonClutterRadarObjects(radarObject, numRadarObjects, egoSpeed, laneBoundaries)
% The radar objects include many objects that belong to the clutter.
% Clutter is defined as a stationary object that is not in front of the
% car. The following types of objects pass as nonclutter:
%
% # Any object in front of the car
% # Any moving object in the area of interest around the car, including
% objects that move at a lateral speed around the car
% Allocate memory
normVs = zeros(numRadarObjects, 1);
inLane = zeros(numRadarObjects, 1);
inZone = zeros(numRadarObjects, 1);
% Parameters
LaneWidth = 3.6; % What is considered in front of the car
ZoneWidth = 1.7*LaneWidth; % A wider area of interest
minV = 1; % Any object that moves slower than minV is considered stationary
for j = 1:numRadarObjects
[vx, vy] = calculateGroundSpeed(radarObject(j).velocity(1),radarObject(j).velocity(2),egoSpeed);
normVs(j) = norm([vx,vy]);
laneBoundariesAtObject = computeBoundaryModel(laneBoundaries, radarObject(j).position(1));
laneCenter = mean(laneBoundariesAtObject);
inLane(j) = (abs(radarObject(j).position(2) - laneCenter) <= LaneWidth/2);
inZone(j) = (abs(radarObject(j).position(2) - laneCenter) <= max(abs(vy)*2, ZoneWidth));
end
realRadarObjectsIdx = union(...
intersect(find(normVs > minV), find(inZone == 1)), ...
find(inLane == 1));
realRadarObjects = radarObject(realRadarObjectsIdx);
end
calculateGroundSpeed根據相對速度和本車速度計算雷達檢測到的物體的真實速度；
function [Vx,Vy] = calculateGroundSpeed(Vxi,Vyi,egoSpeed)
% Inputs
% (Vxi,Vyi) : relative object speed
% egoSpeed : ego vehicle speed
% Outputs
% [Vx,Vy] : ground object speed
Vx = Vxi + egoSpeed; % Calculate longitudinal ground speed
theta = atan2(Vyi,Vxi); % Calculate heading angle
Vy = Vx * tan(theta); % Calculate lateral ground speed
end
processVideo轉換視覺檢測結果爲objectDetection物體列表。
function postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)
% Process the video objects into objectDetection objects
numRadarObjects = numel(postProcessedDetections);
numVisionObjects = visionFrame.numObjects;
if numVisionObjects
classToUse = class(visionFrame.object(1).position);
visionMeasCov = cast(diag([2,2,2,100]), classToUse);
% Process Vision Objects:
for i=1:numVisionObjects
object = visionFrame.object(i);
postProcessedDetections{numRadarObjects+i} = objectDetection(t,...
[object.position(1); object.velocity(1); object.position(2); 0], ...
'SensorIndex', 1, 'MeasurementNoise', visionMeasCov, ...
'MeasurementParameters', {1}, ...
'ObjectClassID', object.classification, ...
'ObjectAttributes', {object.id, object.size});
end
end
end
processRadar轉換雷達檢測結果爲objectDetection物體列表。
function postProcessedDetections = processRadar(postProcessedDetections, realRadarObjects, t)
% Process the radar objects into objectDetection objects
numRadarObjects = numel(realRadarObjects);
if numRadarObjects
classToUse = class(realRadarObjects(1).position);
radarMeasCov = cast(diag([2,2,2,100]), classToUse);
% Process Radar Objects:
for i=1:numRadarObjects
object = realRadarObjects(i);
postProcessedDetections{i} = objectDetection(t, ...
[object.position(1); object.velocity(1); object.position(2); object.velocity(2)], ...
'SensorIndex', 2, 'MeasurementNoise', radarMeasCov, ...
'MeasurementParameters', {2}, ...
'ObjectAttributes', {object.id, object.status, object.amplitude, object.rangeMode});
end
end
end
fcwmeas爲FCW例程中的測量函數。
function measurement = fcwmeas(state, sensorID)
% The example 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].
% sensorID=2: radar objects, the measurement is [x;vx;y;vy].
% The state is:
% Constant velocity state = [x;vx;y;vy]
% Constant turn state = [x;vx;y;vy;omega]
% Constant acceleration state = [x;vx;ax;y;vy;ay]
if numel(state) < 6 % Constant turn or constant velocity
switch sensorID
case 1 % video
measurement = [state(1:3); 0];
case 2 % radar
measurement = state(1:4);
end
else % Constant acceleration
switch sensorID
case 1 % video
measurement = [state(1:2); state(4); 0];
case 2 % radar
measurement = [state(1:2); state(4:5)];
end
end
end
fcwmeasjac爲FCW例程中雅各布行列式測量函數。
function jacobian = fcwmeasjac(state, sensorID)
% The example measurements depend on the sensor type, which is reported by
% the MeasurementParameters property of the objectDetection. We choose
% sensorID=1 for video objects and sensorID=2 for radar objects. The
% following two sensorID values are used:
% sensorID=1: video objects, the measurement is [x;vx;y].
% sensorID=2: radar objects, the measurement is [x;vx;y;vy].
% The state is:
% Constant velocity state = [x;vx;y;vy]
% Constant turn state = [x;vx;y;vy;omega]
% Constant acceleration state = [x;vx;ax;y;vy;ay]
numStates = numel(state);
jacobian = zeros(4, numStates);
if numel(state) < 6 % Constant turn or constant velocity
switch sensorID
case 1 % video
jacobian(1,1) = 1;
jacobian(2,2) = 1;
jacobian(3,3) = 1;
case 2 % radar
jacobian(1,1) = 1;
jacobian(2,2) = 1;
jacobian(3,3) = 1;
jacobian(4,4) = 1;
end
else % Constant acceleration
switch sensorID
case 1 % video
jacobian(1,1) = 1;
jacobian(2,2) = 1;
jacobian(3,4) = 1;
case 2 % radar
jacobian(1,1) = 1;
jacobian(2,2) = 1;
jacobian(3,4) = 1;
jacobian(4,5) = 1;
end
end
end