优化差速转舵并增加纵向辨识测试
This commit is contained in:
@@ -0,0 +1,651 @@
|
||||
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
|
||||
Reference in New Issue
Block a user