652 lines
27 KiB
Matlab
652 lines
27 KiB
Matlab
|
function result = prepare_steering_iddata(experimentRoot, outputDirectory)
|
|||
|
|
%PREPARE_STEERING_IDDATA Convert steering snapshot CSV files to MATLAB iddata.
|
|||
|
|
% RESULT = PREPARE_STEERING_IDDATA() reads the two experiment folders next
|
|||
|
|
% to this file, detects every steering target step, aligns command and
|
|||
|
|
% feedback timestamps, resamples each response to 0.04 s, and saves:
|
|||
|
|
%
|
|||
|
|
% dataEstLF/LR/RF/RR uDiff -> delta actual steering angle
|
|||
|
|
% dataValLF/LR/RF/RR independent validation data
|
|||
|
|
% closedLoopEst* delta target angle -> delta actual angle
|
|||
|
|
% closedLoopVal* independent closed-loop validation data
|
|||
|
|
%
|
|||
|
|
% Each variable is a multi-experiment iddata object. Import one wheel at a
|
|||
|
|
% time into the System Identification app. The first experiment folder
|
|||
|
|
% (Kp=0.0035, dead zone=0.1 deg) is used for estimation and the third
|
|||
|
|
% folder (Kp=0.003, dead zone=0.1 deg) for validation. The second folder
|
|||
|
|
% with a 0.5 deg dead zone is retained as supplementary data and is not
|
|||
|
|
% used by this primary identification workflow. Raw CSV files are never
|
|||
|
|
% modified.
|
|||
|
|
%
|
|||
|
|
% Example:
|
|||
|
|
% result = prepare_steering_iddata;
|
|||
|
|
% load(result.MatFile);
|
|||
|
|
% systemIdentification;
|
|||
|
|
%
|
|||
|
|
% Units: uDiff in m/s, steering angle in degrees, time in seconds.
|
|||
|
|
|
|||
|
|
if nargin < 1 || isempty(experimentRoot)
|
|||
|
|
experimentRoot = fileparts(mfilename('fullpath'));
|
|||
|
|
end
|
|||
|
|
if nargin < 2 || isempty(outputDirectory)
|
|||
|
|
outputDirectory = fullfile(experimentRoot, 'matlab_ready');
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
if exist('iddata', 'file') ~= 2
|
|||
|
|
error('prepare_steering_iddata:MissingToolbox', ...
|
|||
|
|
['找不到 iddata。请安装或启用 MATLAB System Identification ' ...
|
|||
|
|
'Toolbox 后重新运行。']);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
settings = struct();
|
|||
|
|
settings.SampleTimeSeconds = 0.04;
|
|||
|
|
settings.PreStepSeconds = 0.8;
|
|||
|
|
settings.PostStepSeconds = 4.0;
|
|||
|
|
settings.BaselineSeconds = 0.5;
|
|||
|
|
settings.NextStepGuardSeconds = 0.25;
|
|||
|
|
settings.MinimumTargetStepDegrees = 5.0;
|
|||
|
|
settings.MinimumSegmentSamples = 30;
|
|||
|
|
settings.EstimationFolder = '第一次0.0035--死区0.1度';
|
|||
|
|
settings.ValidationFolder = '第三次0.0030--死区0.1度';
|
|||
|
|
|
|||
|
|
wheels = steeringWheelDefinitions();
|
|||
|
|
estimationPath = fullfile(experimentRoot, settings.EstimationFolder);
|
|||
|
|
validationPath = fullfile(experimentRoot, settings.ValidationFolder);
|
|||
|
|
requireFolder(estimationPath);
|
|||
|
|
requireFolder(validationPath);
|
|||
|
|
|
|||
|
|
fprintf('读取估计数据：%s\n', estimationPath);
|
|||
|
|
estimationSegments = processExperimentFolder( ...
|
|||
|
|
estimationPath, 'Est', wheels, settings);
|
|||
|
|
fprintf('读取验证数据：%s\n', validationPath);
|
|||
|
|
validationSegments = processExperimentFolder( ...
|
|||
|
|
validationPath, 'Val', wheels, settings);
|
|||
|
|
|
|||
|
|
allSegments = [estimationSegments, validationSegments];
|
|||
|
|
if isempty(allSegments)
|
|||
|
|
error('prepare_steering_iddata:NoSegments', ...
|
|||
|
|
'没有找到满足条件的舵角切换片段。');
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
dataEstLF = buildIdData(estimationSegments, 'LF', 'plant', settings);
|
|||
|
|
dataEstLR = buildIdData(estimationSegments, 'LR', 'plant', settings);
|
|||
|
|
dataEstRF = buildIdData(estimationSegments, 'RF', 'plant', settings);
|
|||
|
|
dataEstRR = buildIdData(estimationSegments, 'RR', 'plant', settings);
|
|||
|
|
|
|||
|
|
dataValLF = buildIdData(validationSegments, 'LF', 'plant', settings);
|
|||
|
|
dataValLR = buildIdData(validationSegments, 'LR', 'plant', settings);
|
|||
|
|
dataValRF = buildIdData(validationSegments, 'RF', 'plant', settings);
|
|||
|
|
dataValRR = buildIdData(validationSegments, 'RR', 'plant', settings);
|
|||
|
|
|
|||
|
|
closedLoopEstLF = buildIdData( ...
|
|||
|
|
estimationSegments, 'LF', 'closedLoop', settings);
|
|||
|
|
closedLoopEstLR = buildIdData( ...
|
|||
|
|
estimationSegments, 'LR', 'closedLoop', settings);
|
|||
|
|
closedLoopEstRF = buildIdData( ...
|
|||
|
|
estimationSegments, 'RF', 'closedLoop', settings);
|
|||
|
|
closedLoopEstRR = buildIdData( ...
|
|||
|
|
estimationSegments, 'RR', 'closedLoop', settings);
|
|||
|
|
|
|||
|
|
closedLoopValLF = buildIdData( ...
|
|||
|
|
validationSegments, 'LF', 'closedLoop', settings);
|
|||
|
|
closedLoopValLR = buildIdData( ...
|
|||
|
|
validationSegments, 'LR', 'closedLoop', settings);
|
|||
|
|
closedLoopValRF = buildIdData( ...
|
|||
|
|
validationSegments, 'RF', 'closedLoop', settings);
|
|||
|
|
closedLoopValRR = buildIdData( ...
|
|||
|
|
validationSegments, 'RR', 'closedLoop', settings);
|
|||
|
|
|
|||
|
|
metadata = buildMetadataTable(allSegments);
|
|||
|
|
samples = buildSampleTable(allSegments);
|
|||
|
|
|
|||
|
|
if ~exist(outputDirectory, 'dir')
|
|||
|
|
mkdir(outputDirectory);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
matFile = fullfile(outputDirectory, 'steering_identification_data.mat');
|
|||
|
|
metadataFile = fullfile(outputDirectory, 'steering_segment_metadata.csv');
|
|||
|
|
samplesFile = fullfile(outputDirectory, 'steering_identification_samples.csv');
|
|||
|
|
|
|||
|
|
save(matFile, ...
|
|||
|
|
'dataEstLF', 'dataEstLR', 'dataEstRF', 'dataEstRR', ...
|
|||
|
|
'dataValLF', 'dataValLR', 'dataValRF', 'dataValRR', ...
|
|||
|
|
'closedLoopEstLF', 'closedLoopEstLR', ...
|
|||
|
|
'closedLoopEstRF', 'closedLoopEstRR', ...
|
|||
|
|
'closedLoopValLF', 'closedLoopValLR', ...
|
|||
|
|
'closedLoopValRF', 'closedLoopValRR', ...
|
|||
|
|
'metadata', 'settings', '-v7.3');
|
|||
|
|
writetable(metadata, metadataFile);
|
|||
|
|
writetable(samples, samplesFile);
|
|||
|
|
|
|||
|
|
result = struct();
|
|||
|
|
result.MatFile = matFile;
|
|||
|
|
result.MetadataFile = metadataFile;
|
|||
|
|
result.SamplesFile = samplesFile;
|
|||
|
|
result.EstimationSegmentCount = numel(estimationSegments);
|
|||
|
|
result.ValidationSegmentCount = numel(validationSegments);
|
|||
|
|
result.SampleTimeSeconds = settings.SampleTimeSeconds;
|
|||
|
|
|
|||
|
|
fprintf('\n处理完成。每个舵轮单独建立多实验数据：\n');
|
|||
|
|
printSegmentCounts(estimationSegments, validationSegments, wheels);
|
|||
|
|
fprintf('MATLAB辨识数据：%s\n', matFile);
|
|||
|
|
fprintf('片段元数据：%s\n', metadataFile);
|
|||
|
|
fprintf('长表CSV：%s\n', samplesFile);
|
|||
|
|
fprintf(['\n在工作区加载 MAT 文件后，将 dataEstLF 等变量作为估计数据，' ...
|
|||
|
|
'将对应的 dataValLF 等变量作为验证数据。\n']);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function wheels = steeringWheelDefinitions()
|
|||
|
|
%STEERINGWHEELDEFINITIONS Describe the four differential steering modules.
|
|||
|
|
wheels = struct( ...
|
|||
|
|
'Code', {'LF', 'LR', 'RF', 'RR'}, ...
|
|||
|
|
'Name', {'LeftFront', 'LeftRear', 'RightFront', 'RightRear'}, ...
|
|||
|
|
'TargetColumn', { ...
|
|||
|
|
'TargetThLeftFront', 'TargetThLeftRear', ...
|
|||
|
|
'TargetThRightFront', 'TargetThRightRear'}, ...
|
|||
|
|
'ActualColumn', { ...
|
|||
|
|
'ActualThLeftFront', 'ActualThLeftRear', ...
|
|||
|
|
'ActualThRightFront', 'ActualThRightRear'}, ...
|
|||
|
|
'ActualTimeColumn', { ...
|
|||
|
|
'ActualThLeftFrontReceiveElapsedMs', ...
|
|||
|
|
'ActualThLeftRearReceiveElapsedMs', ...
|
|||
|
|
'ActualThRightFrontReceiveElapsedMs', ...
|
|||
|
|
'ActualThRightRearReceiveElapsedMs'}, ...
|
|||
|
|
'SentLeftColumn', { ...
|
|||
|
|
'SentLFLMps', 'SentLRLMps', 'SentRFLMps', 'SentRRLMps'}, ...
|
|||
|
|
'SentRightColumn', { ...
|
|||
|
|
'SentLFRMps', 'SentLRRMps', 'SentRFRMps', 'SentRRRMps'}, ...
|
|||
|
|
'LeftLimitedColumn', { ...
|
|||
|
|
'CommandLimitedLFL', 'CommandLimitedLRL', ...
|
|||
|
|
'CommandLimitedRFL', 'CommandLimitedRRL'}, ...
|
|||
|
|
'RightLimitedColumn', { ...
|
|||
|
|
'CommandLimitedLFR', 'CommandLimitedLRR', ...
|
|||
|
|
'CommandLimitedRFR', 'CommandLimitedRRR'}, ...
|
|||
|
|
'PairLimitedColumn', { ...
|
|||
|
|
'PairCommandLimitedLeftFront', ...
|
|||
|
|
'PairCommandLimitedLeftRear', ...
|
|||
|
|
'PairCommandLimitedRightFront', ...
|
|||
|
|
'PairCommandLimitedRightRear'});
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function segments = processExperimentFolder(folder, datasetCode, wheels, settings)
|
|||
|
|
%PROCESSEXPERIMENTFOLDER Convert all snapshot files in one experiment set.
|
|||
|
|
files = dir(fullfile(folder, '*_snapshot.csv'));
|
|||
|
|
if isempty(files)
|
|||
|
|
error('prepare_steering_iddata:NoSnapshotFiles', ...
|
|||
|
|
'目录中没有 *_snapshot.csv：%s', folder);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
[~, order] = sort({files.name});
|
|||
|
|
files = files(order);
|
|||
|
|
segments = emptySegmentArray();
|
|||
|
|
|
|||
|
|
for fileIndex = 1:numel(files)
|
|||
|
|
sourcePath = fullfile(files(fileIndex).folder, files(fileIndex).name);
|
|||
|
|
fprintf(' %s\n', files(fileIndex).name);
|
|||
|
|
snapshot = readNumericSnapshot(sourcePath);
|
|||
|
|
validateSnapshotColumns(snapshot, wheels);
|
|||
|
|
|
|||
|
|
transitionType = regexprep( ...
|
|||
|
|
files(fileIndex).name, '_Car\d+_snapshot\.csv$', '');
|
|||
|
|
fileSegments = processSnapshot( ...
|
|||
|
|
snapshot, files(fileIndex).name, transitionType, ...
|
|||
|
|
datasetCode, wheels, settings);
|
|||
|
|
segments = [segments, fileSegments]; %#ok<AGROW>
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function snapshot = readNumericSnapshot(sourcePath)
|
|||
|
|
%READNUMERICSNAPSHOT Read the numeric diagnostic snapshot without mutation.
|
|||
|
|
importOptions = detectImportOptions(sourcePath, 'Delimiter', ',');
|
|||
|
|
importOptions = setvartype( ...
|
|||
|
|
importOptions, importOptions.VariableNames, 'double');
|
|||
|
|
snapshot = readtable(sourcePath, importOptions);
|
|||
|
|
if isempty(snapshot)
|
|||
|
|
error('prepare_steering_iddata:EmptySnapshot', ...
|
|||
|
|
'CSV中没有数据：%s', sourcePath);
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function validateSnapshotColumns(snapshot, wheels)
|
|||
|
|
%VALIDATESNAPSHOTCOLUMNS Ensure every signal needed for identification exists.
|
|||
|
|
required = { ...
|
|||
|
|
'ElapsedMs', 'ControlElapsedMs', 'WheelCommandElapsedMs', ...
|
|||
|
|
'WheelCommandSuppressed', 'DiffSteerKp', 'DiffSteerKi', ...
|
|||
|
|
'DiffSteerKd', 'DiffSteerDeadZone', ...
|
|||
|
|
'DiffSteerRateFeedforwardGain'};
|
|||
|
|
|
|||
|
|
for wheelIndex = 1:numel(wheels)
|
|||
|
|
wheel = wheels(wheelIndex);
|
|||
|
|
required = [required, { ... %#ok<AGROW>
|
|||
|
|
wheel.TargetColumn, wheel.ActualColumn, ...
|
|||
|
|
wheel.ActualTimeColumn, wheel.SentLeftColumn, ...
|
|||
|
|
wheel.SentRightColumn, wheel.LeftLimitedColumn, ...
|
|||
|
|
wheel.RightLimitedColumn, wheel.PairLimitedColumn}];
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
missing = setdiff(required, snapshot.Properties.VariableNames);
|
|||
|
|
if ~isempty(missing)
|
|||
|
|
error('prepare_steering_iddata:MissingColumns', ...
|
|||
|
|
'snapshot CSV缺少字段：%s', strjoin(missing, ', '));
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function segments = processSnapshot( ...
|
|||
|
|
snapshot, sourceFile, transitionType, datasetCode, wheels, settings)
|
|||
|
|
%PROCESSSNAPSHOT Extract one iddata experiment for every wheel target step.
|
|||
|
|
elapsedMs = snapshot.ElapsedMs;
|
|||
|
|
finiteElapsed = elapsedMs(isfinite(elapsedMs));
|
|||
|
|
if isempty(finiteElapsed)
|
|||
|
|
error('prepare_steering_iddata:InvalidTime', ...
|
|||
|
|
'%s没有有效ElapsedMs。', sourceFile);
|
|||
|
|
end
|
|||
|
|
timeOriginMs = finiteElapsed(1);
|
|||
|
|
|
|||
|
|
kp = constantParameter(snapshot.DiffSteerKp, 'DiffSteerKp', sourceFile);
|
|||
|
|
ki = constantParameter(snapshot.DiffSteerKi, 'DiffSteerKi', sourceFile);
|
|||
|
|
kd = constantParameter(snapshot.DiffSteerKd, 'DiffSteerKd', sourceFile);
|
|||
|
|
deadZone = constantParameter( ...
|
|||
|
|
snapshot.DiffSteerDeadZone, 'DiffSteerDeadZone', sourceFile);
|
|||
|
|
feedforwardGain = constantParameter( ...
|
|||
|
|
snapshot.DiffSteerRateFeedforwardGain, ...
|
|||
|
|
'DiffSteerRateFeedforwardGain', sourceFile);
|
|||
|
|
|
|||
|
|
segments = emptySegmentArray();
|
|||
|
|
for wheelIndex = 1:numel(wheels)
|
|||
|
|
wheel = wheels(wheelIndex);
|
|||
|
|
wheelSegments = extractWheelSegments( ...
|
|||
|
|
snapshot, timeOriginMs, sourceFile, transitionType, ...
|
|||
|
|
datasetCode, wheel, settings, kp, ki, kd, ...
|
|||
|
|
deadZone, feedforwardGain);
|
|||
|
|
segments = [segments, wheelSegments]; %#ok<AGROW>
|
|||
|
|
|
|||
|
|
if numel(wheelSegments) ~= 20
|
|||
|
|
warning('prepare_steering_iddata:UnexpectedStepCount', ...
|
|||
|
|
'%s %s检测到%d次切换，预期为20次。', ...
|
|||
|
|
sourceFile, wheel.Code, numel(wheelSegments));
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function segments = extractWheelSegments( ...
|
|||
|
|
snapshot, timeOriginMs, sourceFile, transitionType, datasetCode, ...
|
|||
|
|
wheel, settings, kp, ki, kd, deadZone, feedforwardGain)
|
|||
|
|
%EXTRACTWHEELSEGMENTS Align and split one steering module's time series.
|
|||
|
|
targetTime = (snapshot.ControlElapsedMs - timeOriginMs) / 1000;
|
|||
|
|
targetAngle = snapshot.(wheel.TargetColumn);
|
|||
|
|
[targetTime, targetAngle] = compactTimedSignal(targetTime, targetAngle);
|
|||
|
|
|
|||
|
|
commandTime = (snapshot.WheelCommandElapsedMs - timeOriginMs) / 1000;
|
|||
|
|
uDiff = (snapshot.(wheel.SentRightColumn) - ...
|
|||
|
|
snapshot.(wheel.SentLeftColumn)) / 2;
|
|||
|
|
[commandTime, uDiff] = compactTimedSignal(commandTime, uDiff);
|
|||
|
|
|
|||
|
|
actualTime = (snapshot.(wheel.ActualTimeColumn) - timeOriginMs) / 1000;
|
|||
|
|
actualAngle = snapshot.(wheel.ActualColumn);
|
|||
|
|
[actualTime, actualAngle] = compactTimedSignal(actualTime, actualAngle);
|
|||
|
|
|
|||
|
|
if numel(targetTime) < 2 || numel(commandTime) < 2 || ...
|
|||
|
|
numel(actualTime) < 2
|
|||
|
|
warning('prepare_steering_iddata:InsufficientSignal', ...
|
|||
|
|
'%s %s的时间序列不足，已跳过。', sourceFile, wheel.Code);
|
|||
|
|
segments = emptySegmentArray();
|
|||
|
|
return;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
rawEventIndices = find( ...
|
|||
|
|
abs(diff(targetAngle)) >= settings.MinimumTargetStepDegrees) + 1;
|
|||
|
|
eventIndices = suppressNearbyEvents( ...
|
|||
|
|
rawEventIndices, targetTime, settings.NextStepGuardSeconds);
|
|||
|
|
segments = emptySegmentArray();
|
|||
|
|
|
|||
|
|
for eventNumber = 1:numel(eventIndices)
|
|||
|
|
targetIndex = eventIndices(eventNumber);
|
|||
|
|
eventTime = targetTime(targetIndex);
|
|||
|
|
targetStep = targetAngle(targetIndex) - targetAngle(targetIndex - 1);
|
|||
|
|
|
|||
|
|
segmentStart = eventTime - settings.PreStepSeconds;
|
|||
|
|
segmentEnd = eventTime + settings.PostStepSeconds;
|
|||
|
|
if eventNumber < numel(eventIndices)
|
|||
|
|
nextEventTime = targetTime(eventIndices(eventNumber + 1));
|
|||
|
|
segmentEnd = min( ...
|
|||
|
|
segmentEnd, nextEventTime - settings.NextStepGuardSeconds);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
commonStart = max([ ...
|
|||
|
|
segmentStart, targetTime(1), commandTime(1), actualTime(1)]);
|
|||
|
|
commonEnd = min([ ...
|
|||
|
|
segmentEnd, targetTime(end), commandTime(end), actualTime(end)]);
|
|||
|
|
firstGridIndex = ceil(commonStart / settings.SampleTimeSeconds);
|
|||
|
|
lastGridIndex = floor(commonEnd / settings.SampleTimeSeconds);
|
|||
|
|
commonTime = (firstGridIndex:lastGridIndex)' * ...
|
|||
|
|
settings.SampleTimeSeconds;
|
|||
|
|
|
|||
|
|
if numel(commonTime) < settings.MinimumSegmentSamples
|
|||
|
|
warning('prepare_steering_iddata:ShortSegment', ...
|
|||
|
|
'%s %s第%d次切换数据过短，已跳过。', ...
|
|||
|
|
sourceFile, wheel.Code, eventNumber);
|
|||
|
|
continue;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
targetResampled = interp1( ...
|
|||
|
|
targetTime, targetAngle, commonTime, 'previous');
|
|||
|
|
inputResampled = interp1( ...
|
|||
|
|
commandTime, uDiff, commonTime, 'previous');
|
|||
|
|
outputResampled = interp1( ...
|
|||
|
|
actualTime, actualAngle, commonTime, 'linear');
|
|||
|
|
relativeTime = commonTime - eventTime;
|
|||
|
|
|
|||
|
|
baselineMask = relativeTime >= -settings.BaselineSeconds & ...
|
|||
|
|
relativeTime < -settings.SampleTimeSeconds / 2;
|
|||
|
|
if ~any(baselineMask)
|
|||
|
|
baselineMask = relativeTime < 0;
|
|||
|
|
end
|
|||
|
|
if ~any(baselineMask)
|
|||
|
|
warning('prepare_steering_iddata:MissingBaseline', ...
|
|||
|
|
'%s %s第%d次切换没有切换前基线，已跳过。', ...
|
|||
|
|
sourceFile, wheel.Code, eventNumber);
|
|||
|
|
continue;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
inputBaseline = finiteMedian(inputResampled(baselineMask));
|
|||
|
|
outputBaseline = finiteMedian(outputResampled(baselineMask));
|
|||
|
|
targetBaseline = finiteMedian(targetResampled(baselineMask));
|
|||
|
|
inputDelta = inputResampled - inputBaseline;
|
|||
|
|
outputDelta = outputResampled - outputBaseline;
|
|||
|
|
targetDelta = targetResampled - targetBaseline;
|
|||
|
|
|
|||
|
|
valid = isfinite(relativeTime) & isfinite(inputDelta) & ...
|
|||
|
|
isfinite(outputDelta) & isfinite(targetDelta);
|
|||
|
|
if nnz(valid) < settings.MinimumSegmentSamples
|
|||
|
|
warning('prepare_steering_iddata:InvalidSegment', ...
|
|||
|
|
'%s %s第%d次切换有效样本不足，已跳过。', ...
|
|||
|
|
sourceFile, wheel.Code, eventNumber);
|
|||
|
|
continue;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
relativeTime = relativeTime(valid);
|
|||
|
|
inputDelta = inputDelta(valid);
|
|||
|
|
outputDelta = outputDelta(valid);
|
|||
|
|
targetDelta = targetDelta(valid);
|
|||
|
|
|
|||
|
|
if segmentContainsUnsafeCommand( ...
|
|||
|
|
snapshot, timeOriginMs, commonStart, commonEnd, wheel)
|
|||
|
|
warning('prepare_steering_iddata:UnsafeCommandSegment', ...
|
|||
|
|
['%s %s第%d次切换出现命令限幅或安全抑制，' ...
|
|||
|
|
'为保持线性辨识条件已跳过。'], ...
|
|||
|
|
sourceFile, wheel.Code, eventNumber);
|
|||
|
|
continue;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
experimentName = sprintf('%s_%s_%s_E%02d', ...
|
|||
|
|
datasetCode, transitionType, wheel.Code, eventNumber);
|
|||
|
|
segment = struct();
|
|||
|
|
segment.Dataset = datasetCode;
|
|||
|
|
segment.Wheel = wheel.Code;
|
|||
|
|
segment.SourceFile = sourceFile;
|
|||
|
|
segment.TransitionType = transitionType;
|
|||
|
|
segment.EventIndex = eventNumber;
|
|||
|
|
segment.ExperimentName = experimentName;
|
|||
|
|
segment.EventTimeSeconds = eventTime;
|
|||
|
|
segment.TargetStepDegrees = targetStep;
|
|||
|
|
segment.Kp = kp;
|
|||
|
|
segment.Ki = ki;
|
|||
|
|
segment.Kd = kd;
|
|||
|
|
segment.DeadZoneDegrees = deadZone;
|
|||
|
|
segment.FeedforwardGain = feedforwardGain;
|
|||
|
|
segment.InputBaselineMps = inputBaseline;
|
|||
|
|
segment.OutputBaselineDegrees = outputBaseline;
|
|||
|
|
segment.TimeSeconds = relativeTime;
|
|||
|
|
segment.UDiffMps = inputDelta;
|
|||
|
|
segment.ActualAngleDeltaDegrees = outputDelta;
|
|||
|
|
segment.TargetAngleDeltaDegrees = targetDelta;
|
|||
|
|
segments(end + 1) = segment; %#ok<AGROW>
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function [time, value] = compactTimedSignal(time, value)
|
|||
|
|
%COMPACTTIMEDSIGNAL Remove missing data and keep the last value per timestamp.
|
|||
|
|
valid = isfinite(time) & isfinite(value);
|
|||
|
|
time = time(valid);
|
|||
|
|
value = value(valid);
|
|||
|
|
[time, order] = sort(time);
|
|||
|
|
value = value(order);
|
|||
|
|
[time, uniqueIndices] = unique(time, 'last');
|
|||
|
|
value = value(uniqueIndices);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function eventIndices = suppressNearbyEvents(rawIndices, time, minimumGap)
|
|||
|
|
%SUPPRESSNEARBYEVENTS Treat rapid intermediate target updates as one step.
|
|||
|
|
eventIndices = zeros(0, 1);
|
|||
|
|
for index = 1:numel(rawIndices)
|
|||
|
|
candidate = rawIndices(index);
|
|||
|
|
if isempty(eventIndices) || ...
|
|||
|
|
time(candidate) - time(eventIndices(end)) >= minimumGap
|
|||
|
|
eventIndices(end + 1, 1) = candidate; %#ok<AGROW>
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function unsafe = segmentContainsUnsafeCommand( ...
|
|||
|
|
snapshot, timeOriginMs, startTime, endTime, wheel)
|
|||
|
|
%SEGMENTCONTAINSUNSAFECOMMAND Reject saturated or suppressed responses.
|
|||
|
|
snapshotTime = (snapshot.ElapsedMs - timeOriginMs) / 1000;
|
|||
|
|
inSegment = snapshotTime >= startTime & snapshotTime <= endTime;
|
|||
|
|
unsafe = any(snapshot.WheelCommandSuppressed(inSegment) ~= 0) || ...
|
|||
|
|
any(snapshot.(wheel.LeftLimitedColumn)(inSegment) ~= 0) || ...
|
|||
|
|
any(snapshot.(wheel.RightLimitedColumn)(inSegment) ~= 0) || ...
|
|||
|
|
any(snapshot.(wheel.PairLimitedColumn)(inSegment) ~= 0);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function value = constantParameter(values, parameterName, sourceFile)
|
|||
|
|
%CONSTANTPARAMETER Verify that one experiment did not change configuration.
|
|||
|
|
finiteValues = values(isfinite(values));
|
|||
|
|
if isempty(finiteValues)
|
|||
|
|
error('prepare_steering_iddata:MissingParameter', ...
|
|||
|
|
'%s没有有效的%s。', sourceFile, parameterName);
|
|||
|
|
end
|
|||
|
|
value = finiteMedian(finiteValues);
|
|||
|
|
tolerance = max(1e-9, abs(value) * 1e-6);
|
|||
|
|
if any(abs(finiteValues - value) > tolerance)
|
|||
|
|
error('prepare_steering_iddata:ChangingParameter', ...
|
|||
|
|
'%s中的%s在记录期间发生变化，不能作为单一实验。', ...
|
|||
|
|
sourceFile, parameterName);
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function value = finiteMedian(values)
|
|||
|
|
%FINITEMEDIAN Return the median after removing non-finite samples.
|
|||
|
|
values = values(isfinite(values));
|
|||
|
|
if isempty(values)
|
|||
|
|
value = NaN;
|
|||
|
|
else
|
|||
|
|
value = median(values);
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function data = buildIdData(segments, wheelCode, inputKind, settings)
|
|||
|
|
%BUILDIDDATA Build one multi-experiment iddata object for one steering module.
|
|||
|
|
selected = segments(strcmp({segments.Wheel}, wheelCode));
|
|||
|
|
if isempty(selected)
|
|||
|
|
error('prepare_steering_iddata:MissingWheelData', ...
|
|||
|
|
'没有找到%s舵轮的%s数据。', wheelCode, inputKind);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
outputs = cell(1, numel(selected));
|
|||
|
|
inputs = cell(1, numel(selected));
|
|||
|
|
experimentNames = cell(1, numel(selected));
|
|||
|
|
for index = 1:numel(selected)
|
|||
|
|
outputs{index} = selected(index).ActualAngleDeltaDegrees(:);
|
|||
|
|
if strcmp(inputKind, 'plant')
|
|||
|
|
inputs{index} = selected(index).UDiffMps(:);
|
|||
|
|
else
|
|||
|
|
inputs{index} = selected(index).TargetAngleDeltaDegrees(:);
|
|||
|
|
end
|
|||
|
|
experimentNames{index} = selected(index).ExperimentName;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
data = iddata(outputs, inputs, settings.SampleTimeSeconds);
|
|||
|
|
data.ExperimentName = experimentNames;
|
|||
|
|
data.OutputName = {'actualSteeringAngleDelta'};
|
|||
|
|
data.OutputUnit = {'deg'};
|
|||
|
|
data.TimeUnit = 'seconds';
|
|||
|
|
if strcmp(inputKind, 'plant')
|
|||
|
|
data.Name = sprintf('%s steering plant: uDiff to angle', wheelCode);
|
|||
|
|
data.InputName = {'uDiff'};
|
|||
|
|
data.InputUnit = {'m/s'};
|
|||
|
|
else
|
|||
|
|
data.Name = sprintf('%s existing closed loop: target to angle', wheelCode);
|
|||
|
|
data.InputName = {'targetSteeringAngleDelta'};
|
|||
|
|
data.InputUnit = {'deg'};
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function metadata = buildMetadataTable(segments)
|
|||
|
|
%BUILDMETADATATABLE Create one row of traceability data per step response.
|
|||
|
|
count = numel(segments);
|
|||
|
|
dataset = cell(count, 1);
|
|||
|
|
wheel = cell(count, 1);
|
|||
|
|
sourceFile = cell(count, 1);
|
|||
|
|
transitionType = cell(count, 1);
|
|||
|
|
experimentName = cell(count, 1);
|
|||
|
|
eventIndex = zeros(count, 1);
|
|||
|
|
eventTimeSeconds = zeros(count, 1);
|
|||
|
|
targetStepDegrees = zeros(count, 1);
|
|||
|
|
kp = zeros(count, 1);
|
|||
|
|
ki = zeros(count, 1);
|
|||
|
|
kd = zeros(count, 1);
|
|||
|
|
deadZoneDegrees = zeros(count, 1);
|
|||
|
|
feedforwardGain = zeros(count, 1);
|
|||
|
|
inputBaselineMps = zeros(count, 1);
|
|||
|
|
outputBaselineDegrees = zeros(count, 1);
|
|||
|
|
sampleCount = zeros(count, 1);
|
|||
|
|
|
|||
|
|
for index = 1:count
|
|||
|
|
segment = segments(index);
|
|||
|
|
dataset{index} = segment.Dataset;
|
|||
|
|
wheel{index} = segment.Wheel;
|
|||
|
|
sourceFile{index} = segment.SourceFile;
|
|||
|
|
transitionType{index} = segment.TransitionType;
|
|||
|
|
experimentName{index} = segment.ExperimentName;
|
|||
|
|
eventIndex(index) = segment.EventIndex;
|
|||
|
|
eventTimeSeconds(index) = segment.EventTimeSeconds;
|
|||
|
|
targetStepDegrees(index) = segment.TargetStepDegrees;
|
|||
|
|
kp(index) = segment.Kp;
|
|||
|
|
ki(index) = segment.Ki;
|
|||
|
|
kd(index) = segment.Kd;
|
|||
|
|
deadZoneDegrees(index) = segment.DeadZoneDegrees;
|
|||
|
|
feedforwardGain(index) = segment.FeedforwardGain;
|
|||
|
|
inputBaselineMps(index) = segment.InputBaselineMps;
|
|||
|
|
outputBaselineDegrees(index) = segment.OutputBaselineDegrees;
|
|||
|
|
sampleCount(index) = numel(segment.TimeSeconds);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
metadata = table( ...
|
|||
|
|
dataset, wheel, sourceFile, transitionType, experimentName, ...
|
|||
|
|
eventIndex, eventTimeSeconds, targetStepDegrees, ...
|
|||
|
|
kp, ki, kd, deadZoneDegrees, feedforwardGain, ...
|
|||
|
|
inputBaselineMps, outputBaselineDegrees, sampleCount, ...
|
|||
|
|
'VariableNames', { ...
|
|||
|
|
'Dataset', 'Wheel', 'SourceFile', 'TransitionType', ...
|
|||
|
|
'ExperimentName', 'EventIndex', 'EventTimeSeconds', ...
|
|||
|
|
'TargetStepDegrees', 'Kp', 'Ki', 'Kd', ...
|
|||
|
|
'DeadZoneDegrees', 'FeedforwardGain', ...
|
|||
|
|
'InputBaselineMps', 'OutputBaselineDegrees', 'SampleCount'});
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function samples = buildSampleTable(segments)
|
|||
|
|
%BUILDSAMPLETABLE Export a tidy long-form table for manual inspection.
|
|||
|
|
totalSamples = sum(arrayfun( ...
|
|||
|
|
@(segment) numel(segment.TimeSeconds), segments));
|
|||
|
|
dataset = cell(totalSamples, 1);
|
|||
|
|
wheel = cell(totalSamples, 1);
|
|||
|
|
experimentName = cell(totalSamples, 1);
|
|||
|
|
sourceFile = cell(totalSamples, 1);
|
|||
|
|
transitionType = cell(totalSamples, 1);
|
|||
|
|
eventIndex = zeros(totalSamples, 1);
|
|||
|
|
timeSeconds = zeros(totalSamples, 1);
|
|||
|
|
uDiffMps = zeros(totalSamples, 1);
|
|||
|
|
actualAngleDeltaDegrees = zeros(totalSamples, 1);
|
|||
|
|
targetAngleDeltaDegrees = zeros(totalSamples, 1);
|
|||
|
|
kp = zeros(totalSamples, 1);
|
|||
|
|
deadZoneDegrees = zeros(totalSamples, 1);
|
|||
|
|
|
|||
|
|
firstRow = 1;
|
|||
|
|
for index = 1:numel(segments)
|
|||
|
|
segment = segments(index);
|
|||
|
|
count = numel(segment.TimeSeconds);
|
|||
|
|
rows = (firstRow:(firstRow + count - 1))';
|
|||
|
|
dataset(rows) = repmat({segment.Dataset}, count, 1);
|
|||
|
|
wheel(rows) = repmat({segment.Wheel}, count, 1);
|
|||
|
|
experimentName(rows) = repmat( ...
|
|||
|
|
{segment.ExperimentName}, count, 1);
|
|||
|
|
sourceFile(rows) = repmat({segment.SourceFile}, count, 1);
|
|||
|
|
transitionType(rows) = repmat( ...
|
|||
|
|
{segment.TransitionType}, count, 1);
|
|||
|
|
eventIndex(rows) = segment.EventIndex;
|
|||
|
|
timeSeconds(rows) = segment.TimeSeconds;
|
|||
|
|
uDiffMps(rows) = segment.UDiffMps;
|
|||
|
|
actualAngleDeltaDegrees(rows) = segment.ActualAngleDeltaDegrees;
|
|||
|
|
targetAngleDeltaDegrees(rows) = segment.TargetAngleDeltaDegrees;
|
|||
|
|
kp(rows) = segment.Kp;
|
|||
|
|
deadZoneDegrees(rows) = segment.DeadZoneDegrees;
|
|||
|
|
firstRow = firstRow + count;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
samples = table( ...
|
|||
|
|
dataset, wheel, experimentName, sourceFile, transitionType, ...
|
|||
|
|
eventIndex, timeSeconds, uDiffMps, ...
|
|||
|
|
actualAngleDeltaDegrees, targetAngleDeltaDegrees, ...
|
|||
|
|
kp, deadZoneDegrees, ...
|
|||
|
|
'VariableNames', { ...
|
|||
|
|
'Dataset', 'Wheel', 'ExperimentName', 'SourceFile', ...
|
|||
|
|
'TransitionType', 'EventIndex', 'TimeSeconds', ...
|
|||
|
|
'UDiffMps', 'ActualAngleDeltaDegrees', ...
|
|||
|
|
'TargetAngleDeltaDegrees', 'Kp', 'DeadZoneDegrees'});
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function segments = emptySegmentArray()
|
|||
|
|
%EMPTYSEGMENTARRAY Return an empty struct with the stable segment schema.
|
|||
|
|
template = struct( ...
|
|||
|
|
'Dataset', '', ...
|
|||
|
|
'Wheel', '', ...
|
|||
|
|
'SourceFile', '', ...
|
|||
|
|
'TransitionType', '', ...
|
|||
|
|
'EventIndex', 0, ...
|
|||
|
|
'ExperimentName', '', ...
|
|||
|
|
'EventTimeSeconds', 0, ...
|
|||
|
|
'TargetStepDegrees', 0, ...
|
|||
|
|
'Kp', 0, ...
|
|||
|
|
'Ki', 0, ...
|
|||
|
|
'Kd', 0, ...
|
|||
|
|
'DeadZoneDegrees', 0, ...
|
|||
|
|
'FeedforwardGain', 0, ...
|
|||
|
|
'InputBaselineMps', 0, ...
|
|||
|
|
'OutputBaselineDegrees', 0, ...
|
|||
|
|
'TimeSeconds', zeros(0, 1), ...
|
|||
|
|
'UDiffMps', zeros(0, 1), ...
|
|||
|
|
'ActualAngleDeltaDegrees', zeros(0, 1), ...
|
|||
|
|
'TargetAngleDeltaDegrees', zeros(0, 1));
|
|||
|
|
segments = template([]);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function printSegmentCounts(estimationSegments, validationSegments, wheels)
|
|||
|
|
%PRINTSEGMENTCOUNTS Report the experiment count for every output iddata pair.
|
|||
|
|
for wheelIndex = 1:numel(wheels)
|
|||
|
|
wheelCode = wheels(wheelIndex).Code;
|
|||
|
|
estimationCount = nnz(strcmp( ...
|
|||
|
|
{estimationSegments.Wheel}, wheelCode));
|
|||
|
|
validationCount = nnz(strcmp( ...
|
|||
|
|
{validationSegments.Wheel}, wheelCode));
|
|||
|
|
fprintf(' %s：估计%d段，验证%d段\n', ...
|
|||
|
|
wheelCode, estimationCount, validationCount);
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
function requireFolder(folder)
|
|||
|
|
%REQUIREFOLDER Fail early when the expected experiment folder is absent.
|
|||
|
|
if ~exist(folder, 'dir')
|
|||
|
|
error('prepare_steering_iddata:MissingFolder', ...
|
|||
|
|
'找不到实验目录：%s', folder);
|
|||
|
|
end
|
|||
|
|
end
|