【原】基于MATLAB&SIMULINK开发自动驾驶系统第四十二讲基于多传感器融合的前方碰撞预警

小明师兄 2022-06-19 发布于江苏

展开全文

这个例子展示了如何通过融合视觉和毫米波雷达传感器的数据来执行以跟踪车辆前方的物体，并在危险时触发前向碰撞预警。

42.1 概述

前方碰撞预警(FCW)是驾驶辅助系统和自动驾驶系统中的一项重要功能，其目标是在即将与前车发生碰撞之前，向驾驶员提供正确、及时、可靠的预警。为了实现这一目标，车辆配备了面向前方的视觉和毫米波雷达传感器。为了提高准确警告的概率，并将错误警告的概率降到最低，需要进行传感器融合。

在本例中，一辆测试车（被控车辆）装备了各类传感器，并记录了它们的输出。本例中使用的传感器有：

1 视觉传感器，它提供了观察到的物体列表及其分类和车道边界的信息。对象列表每秒输出10次，即输出帧率为100ms。车道边界每秒输出20次,即输出帧率为50ms。

2 毫米波雷达传感器，具有中远距离模式，提供未分类的观察物体列表。物体列表每秒输出20次，即输出帧率为50ms。

3 IMU，每秒报告被控车辆的速度和转弯率20次，即输出帧率为50ms。

4 摄像机，它记录了车前场景的视频片段。注：这段视频不被跟踪器使用，只用于在视频上显示跟踪结果，以便验证，类似行车记录仪。

提供前方碰撞预警的过程包括以下步骤。

1 获取传感器的数据

2 融合传感器数据，得到轨迹列表，即汽车前方物体的估计位置和速度。

3 根据轨迹和FCW标准发出警告。FCW标准基于Euro-NCAP AEB测试程序，并考虑到与车前物体的相对距离和相对速度。

本例中的可视化是使用monoCamera和birdsEyePlot完成的。为了简洁起见，创建和更新显示的函数被移到本例之外的辅助函数中。

本例是一个脚本，这里显示的是主体，在后面的章节中以局部函数的形式显示帮助例程。有关局部函数的更多细节，请参见 "为脚本添加函数"。

% Setup the display[videoReader,videoDisplayHandle, bepPlotters, sensor] = helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4', 'SensorConfigurationData.mat'); % Readthe recorded detections file[visionObjects,radarObjects, inertialMeasurementUnit, laneReports, ...    timeStep, numSteps] =readSensorRecordingsFile('01_city_c2s_fcw_10s_sensor.mat'); % Aninitial 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 carlaneWidth =3.6; % metersegoLane =struct('left', [0 0 laneWidth/2], 'right', [0 0-laneWidth/2]); %Prepare some time variablestime =0;           % Time since thebeginning of the recordingcurrentStep= 0;    % Current timestepsnapTime =9.3;     % The time to capture asnapshot of the display %Initialize the tracker[tracker,positionSelector, velocitySelector] = setupTracker(); whilecurrentStep < numSteps && ishghandle(videoDisplayHandle)    % Update scenariocounters    currentStep = currentStep + 1;    time = time + timeStep;     % Process the sensordetections as objectDetection inputs to the tracker    [detections, laneBoundaries, egoLane] = processDetections(...        visionObjects(currentStep),radarObjects(currentStep), ...        inertialMeasurementUnit(currentStep),laneReports(currentStep), ...        egoLane, time);     % Using the list ofobjectDetections, return the tracks, updated to time    confirmedTracks = updateTracks(tracker,detections, time);     % Find the most importantobject and calculate the forward collision    % warning    mostImportantObject =findMostImportantObject(confirmedTracks, egoLane, positionSelector,velocitySelector);     % Update video andbirds-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;    endend

42.2 创建多对象跟踪器

multiObjectTracker根据视觉和雷达传感器报告的物体列表，跟踪被控车辆四周的物体。通过融合两个传感器的信息，降低了错误碰撞预警的概率。

setupTracker函数会返回multiObjectTracker。当创建一个multiObjectTracker，考虑如下:

1 FilterInitializationFcn。可能的运动和测量模型。在这种情况下，预计物体会有一个恒定的加速度运动。虽然你可以为这个模型配置一个线性卡尔曼滤波器，但initConstantAccelerationFilter配置了一个扩展的卡尔曼滤波器。参见 "定义卡尔曼滤波器 "部分。

2 AssignmentThreshold: 检测到的数据与轨迹的距离。这个参数的默认值是30。如果有一些检测结果没有被分配到轨道上，但应该被分配到轨道上，则增加该值。如果有检测结果被分配到太远的轨道上，则降低此值。本例使用35。

3 DeletionThreshold。当一条轨道被确认后，不应该在第一次更新时删除，因为没有检测分配给它。相反，它应该被海岸（预测），直到很明显该轨迹没有得到任何传感器信息来更新它。其逻辑是，如果轨道在 Q 次中漏掉 P 次，则应将其删除。该参数的默认值是5次中的5次。在这种情况下，跟踪器每秒被调用20次，而且有两个传感器，所以没有必要修改默认值。

4 ConfirmationThreshold。确认轨迹的参数。每次未分配的检测都会初始化一个新的轨迹。其中有些检测可能是假的，所以所有的轨迹都初始化为 "暂定"。要确认一条轨迹，必须在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, 'ConfirmationThreshold', [2 3], ...            'DeletionThreshold', 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

42.3 定义卡尔曼滤波器

上一节中定义的multiObjectTracker使用本节中定义的过滤器初始化函数来创建一个卡尔曼过滤器（线性、扩展或无痕）。然后，这个滤波器被用于跟踪被控车辆周围的每个对象。

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);    endend

42.4 处理和格式化检测

记录的信息必须经过处理和格式化，才能被跟踪器使用。这有以下几个步骤:

1过滤掉不必要的毫米波雷达干扰探测。毫米波雷达会输出许多与固定物体相对应的物体，这些物体包括：护栏、道路中间线、交通标志等。如果在跟踪中使用了这些检测结果，就会在道路边缘产生固定物体的虚假轨迹，因此必须在调用跟踪器之前将其删除。毫米波雷达物体如果在车前静止或在车附近移动，则被认为是非杂乱物体。

2 将检测结果格式化，作为跟踪器的输入，即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 objects        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

42.5 更新追踪

要更新跟踪，调用updateTracks 方法，输入如下内容。

1 tracker - 之前配置的 multiObjectTracker。

2 detections - 由processDetections创建的objectDetection对象列表。

3 time - 当前场景的时间。

跟踪器的输出是一个轨迹结构数组。

找到最重要的对象并发出前向碰撞警告。

最重要的物体（MIO）被定义为在被控车道上且离车前最近的轨迹，即正x值最小。为了降低误报的概率，只考虑确认的轨迹。

一旦找到了MIO，就会计算汽车和MIO之间的相对速度。相对距离和相对速度决定了前撞预警。FCW有3种情况。

1 安全（绿色）。被控车道上没有车（没有MIO），MIO正在远离汽车，或者与MIO的距离保持不变。

2 小心（黄色）。MIO正在向汽车靠近，但距离仍高于FCW距离。FCW距离使用欧洲NCAP AEB测试协议计算。请注意，该距离随MIO和汽车之间的相对速度而变化，当关闭速度较高时，该距离较大。

3 警告（红色）。MIO正在向汽车靠近，其距离小于FCW距离。

Euro-NCAP AEB测试协议定义了以下距离计算。

其中：

是前方碰撞警告距离。

是两车之间的相对速度。

是最大减速度，定义为重力加速度的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 brake, 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);

42.6 总结

这个例子展示了如何为配备视觉、毫米波雷达和IMU传感器的车辆创建一个前向碰撞预警系统。它使用objectDetection对象将传感器报告传递给multiObjectTracker对象，multiObjectTracker对象将它们融合，并跟踪被控车辆前方的物体。

尝试为跟踪器使用不同的参数，看看它们如何影响跟踪质量。尝试修改跟踪过滤器，使用 trackingKF 或 trackingUKF，或者定义不同的运动模型，例如，恒速或恒转。最后，你可以尝试定义自己的运动模型。

42.7 支持函数

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 millisecondsnumSteps = numel(visionObjects); % Number of recorded timestepsend

processLanes 将传感器报告的车道转换为抛物线型车道边界车道，并保持持久的被控车道估量：

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 lanecond = (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 onesleftParams  = egoLane.left;leftBoundaries = parabolicLaneBoundary(leftParams);leftBoundaries.Strength = 1; % Check the validity of the reported right lanecond = (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 onesrightParams = egoLane.right;rightBoundaries = parabolicLaneBoundary(rightParams);rightBoundaries.Strength = 1; laneBoundaries = [leftBoundaries, rightBoundaries];end

findNonClutterRadarObject 移除被认为是干扰的毫米波雷达对象：

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 objectsnumRadarObjects = 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});    endendend

processRadar 将报告的毫米波雷达对象转换为objectDetection对象列表:

function postProcessedDetections = processRadar(postProcessedDetections, realRadarObjects, t)% Process the radar objects into objectDetection objectsnumRadarObjects = 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});    endendend

fcwmeas 这个前向碰撞预警示例中使用的测量函数:

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    endend

fcwmeasjac 这个前向碰撞预警例子中使用的测量函数的雅各布值:

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    endend