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
|