完善Detour状态估计与轨迹跟踪验证
This commit is contained in:
@@ -24,6 +24,27 @@ namespace MultiWheelC
|
||||
public double DetourX;
|
||||
public double DetourY;
|
||||
public double DetourTheta;
|
||||
public long DetourTickRaw;
|
||||
public double DetourLStep;
|
||||
|
||||
// Detour状态估计内部诊断；偏移量仅在跳变候选有效时有意义。
|
||||
public bool HasDetourStateDiagnostics;
|
||||
public bool DetourJumpCandidateActive;
|
||||
public int DetourJumpCandidateConsistentFrameCount;
|
||||
public double DetourEstimatedShiftDistanceMeters;
|
||||
public double DetourEstimatedShiftHeadingRadians;
|
||||
public int DetourAutomaticFrameShiftCount;
|
||||
public string DetourStateStatusReason;
|
||||
public double DetourDataAgeMilliseconds;
|
||||
public double DetourSourceFrameIntervalMilliseconds;
|
||||
public double DetourMotionPredictionTimestampSeconds;
|
||||
public bool HasDetourInnovationDiagnostics;
|
||||
public double DetourPositionInnovationMeters;
|
||||
public double DetourAllowedPositionInnovationMeters;
|
||||
public double DetourHeadingInnovationRadians;
|
||||
public double DetourAllowedHeadingInnovationRadians;
|
||||
public string DetourLastJumpTriggerReason;
|
||||
public string DetourStateStatus;
|
||||
|
||||
// 车体速度单位为m/s，角速度统一使用rad/s。
|
||||
public float CommandSpeed;
|
||||
@@ -62,6 +83,10 @@ namespace MultiWheelC
|
||||
public double WheelFeedbackRawBodyVyMetersPerSecond;
|
||||
public double WheelFeedbackFilteredBodyVyMetersPerSecond;
|
||||
public bool WheelFeedbackVelocityEstimateValid;
|
||||
public bool HasWheelFeedbackAngularVelocityDiagnostics;
|
||||
public double WheelFeedbackRawBodyOmegaRadiansPerSecond;
|
||||
public double WheelFeedbackFilteredBodyOmegaRadiansPerSecond;
|
||||
public double WheelFeedbackSampleTimestampSeconds;
|
||||
|
||||
// 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
|
||||
public bool HasSteeringDiagnostics;
|
||||
@@ -130,6 +155,8 @@ namespace MultiWheelC
|
||||
private readonly float _referenceDecelerationMetersPerSecondSquared;
|
||||
private readonly int _sampleIntervalMs;
|
||||
private readonly MultiWheelChassis _diagnosticChassis;
|
||||
private readonly WheelFeedbackVehicleStateProvider
|
||||
_diagnosticStateProvider;
|
||||
|
||||
private readonly List<TrackingSample> _samples =
|
||||
new List<TrackingSample>();
|
||||
@@ -203,7 +230,9 @@ namespace MultiWheelC
|
||||
float referenceMotionFrameYawDegrees = 0f,
|
||||
float referenceAccelerationMetersPerSecondSquared = 0f,
|
||||
float referenceDecelerationMetersPerSecondSquared = 0f,
|
||||
MultiWheelChassis diagnosticChassis = null)
|
||||
MultiWheelChassis diagnosticChassis = null,
|
||||
WheelFeedbackVehicleStateProvider
|
||||
diagnosticStateProvider = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(controllerName))
|
||||
throw new ArgumentException(
|
||||
@@ -235,6 +264,7 @@ namespace MultiWheelC
|
||||
referenceDecelerationMetersPerSecondSquared;
|
||||
_sampleIntervalMs = sampleIntervalMs;
|
||||
_diagnosticChassis = diagnosticChassis;
|
||||
_diagnosticStateProvider = diagnosticStateProvider;
|
||||
}
|
||||
|
||||
// 保存成功后的CSV绝对路径；尚未保存时为空。
|
||||
@@ -534,6 +564,47 @@ namespace MultiWheelC
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
|
||||
var hasDetourStateDiagnostics = false;
|
||||
var detourJumpCandidateActive = false;
|
||||
var detourJumpCandidateConsistentFrameCount = 0;
|
||||
var detourEstimatedShiftDistanceMeters = 0.0;
|
||||
var detourEstimatedShiftHeadingRadians = 0.0;
|
||||
var detourAutomaticFrameShiftCount = 0;
|
||||
var detourStateStatusReason = string.Empty;
|
||||
var detourDataAgeMilliseconds = 0.0;
|
||||
var detourSourceFrameIntervalSeconds = 0.0;
|
||||
var detourMotionPredictionTimestampSeconds = 0.0;
|
||||
var hasDetourInnovationDiagnostics = false;
|
||||
var detourPositionInnovationMeters = 0.0;
|
||||
var detourAllowedPositionInnovationMeters = 0.0;
|
||||
var detourHeadingInnovationRadians = 0.0;
|
||||
var detourAllowedHeadingInnovationRadians = 0.0;
|
||||
var detourLastJumpTriggerReason = string.Empty;
|
||||
var detourStateStatus = string.Empty;
|
||||
|
||||
if (_diagnosticStateProvider != null)
|
||||
{
|
||||
hasDetourStateDiagnostics =
|
||||
_diagnosticStateProvider
|
||||
.TryGetLatestDetourDiagnostics(
|
||||
out detourJumpCandidateActive,
|
||||
out detourJumpCandidateConsistentFrameCount,
|
||||
out detourEstimatedShiftDistanceMeters,
|
||||
out detourEstimatedShiftHeadingRadians,
|
||||
out detourAutomaticFrameShiftCount,
|
||||
out detourSourceFrameIntervalSeconds,
|
||||
out detourMotionPredictionTimestampSeconds,
|
||||
out hasDetourInnovationDiagnostics,
|
||||
out detourPositionInnovationMeters,
|
||||
out detourAllowedPositionInnovationMeters,
|
||||
out detourHeadingInnovationRadians,
|
||||
out detourAllowedHeadingInnovationRadians,
|
||||
out detourLastJumpTriggerReason,
|
||||
out detourStateStatus,
|
||||
out detourStateStatusReason,
|
||||
out detourDataAgeMilliseconds);
|
||||
}
|
||||
|
||||
float commandSpeed;
|
||||
float commandVx;
|
||||
float commandVy;
|
||||
@@ -556,6 +627,10 @@ namespace MultiWheelC
|
||||
double wheelFeedbackRawBodyVyMetersPerSecond;
|
||||
double wheelFeedbackFilteredBodyVyMetersPerSecond;
|
||||
bool wheelFeedbackVelocityEstimateValid;
|
||||
var hasWheelFeedbackAngularVelocityDiagnostics = false;
|
||||
var wheelFeedbackRawBodyOmegaRadiansPerSecond = 0.0;
|
||||
var wheelFeedbackFilteredBodyOmegaRadiansPerSecond = 0.0;
|
||||
var wheelFeedbackSampleTimestampSeconds = 0.0;
|
||||
bool hasGcpCommand;
|
||||
double requestedFrontGcpAngleRadians;
|
||||
double requestedRearGcpAngleRadians;
|
||||
@@ -655,6 +730,46 @@ namespace MultiWheelC
|
||||
_commandRearGcpAngleRadians;
|
||||
}
|
||||
|
||||
// 直接从状态源读取最新完整轮组诊断，使原地自转等没有
|
||||
// 控制周期回调的实验也能记录原始/滤波Vw及其采样时间。
|
||||
if (_diagnosticStateProvider != null &&
|
||||
_diagnosticStateProvider
|
||||
.TryGetLatestVelocityDiagnostics(
|
||||
out var directDetourBodyVx,
|
||||
out var directDetourVelocityValid,
|
||||
out var directRawWheelBodyVx,
|
||||
out var directFilteredWheelBodyVx,
|
||||
out var directRawWheelBodyVy,
|
||||
out var directFilteredWheelBodyVy,
|
||||
out var directRawWheelBodyOmega,
|
||||
out var directFilteredWheelBodyOmega,
|
||||
out var directWheelSampleTimestampSeconds,
|
||||
out var directWheelVelocityValid))
|
||||
{
|
||||
hasVelocityDiagnostics = true;
|
||||
detourEstimatedBodyVxMetersPerSecond =
|
||||
directDetourBodyVx;
|
||||
detourVelocityEstimateValid =
|
||||
directDetourVelocityValid;
|
||||
wheelFeedbackRawBodyVxMetersPerSecond =
|
||||
directRawWheelBodyVx;
|
||||
wheelFeedbackFilteredBodyVxMetersPerSecond =
|
||||
directFilteredWheelBodyVx;
|
||||
wheelFeedbackRawBodyVyMetersPerSecond =
|
||||
directRawWheelBodyVy;
|
||||
wheelFeedbackFilteredBodyVyMetersPerSecond =
|
||||
directFilteredWheelBodyVy;
|
||||
wheelFeedbackRawBodyOmegaRadiansPerSecond =
|
||||
directRawWheelBodyOmega;
|
||||
wheelFeedbackFilteredBodyOmegaRadiansPerSecond =
|
||||
directFilteredWheelBodyOmega;
|
||||
wheelFeedbackSampleTimestampSeconds =
|
||||
directWheelSampleTimestampSeconds;
|
||||
wheelFeedbackVelocityEstimateValid =
|
||||
directWheelVelocityValid;
|
||||
hasWheelFeedbackAngularVelocityDiagnostics = true;
|
||||
}
|
||||
|
||||
var sample = new TrackingSample
|
||||
{
|
||||
ElapsedSeconds =
|
||||
@@ -662,6 +777,45 @@ namespace MultiWheelC
|
||||
DetourX = location.x,
|
||||
DetourY = location.y,
|
||||
DetourTheta = location.th,
|
||||
DetourTickRaw = Convert.ToInt64(
|
||||
location.tick,
|
||||
CultureInfo.InvariantCulture),
|
||||
DetourLStep = Convert.ToDouble(
|
||||
location.l_step,
|
||||
CultureInfo.InvariantCulture),
|
||||
HasDetourStateDiagnostics =
|
||||
hasDetourStateDiagnostics,
|
||||
DetourJumpCandidateActive =
|
||||
detourJumpCandidateActive,
|
||||
DetourJumpCandidateConsistentFrameCount =
|
||||
detourJumpCandidateConsistentFrameCount,
|
||||
DetourEstimatedShiftDistanceMeters =
|
||||
detourEstimatedShiftDistanceMeters,
|
||||
DetourEstimatedShiftHeadingRadians =
|
||||
detourEstimatedShiftHeadingRadians,
|
||||
DetourAutomaticFrameShiftCount =
|
||||
detourAutomaticFrameShiftCount,
|
||||
DetourStateStatusReason =
|
||||
detourStateStatusReason,
|
||||
DetourDataAgeMilliseconds =
|
||||
detourDataAgeMilliseconds,
|
||||
DetourSourceFrameIntervalMilliseconds =
|
||||
detourSourceFrameIntervalSeconds * 1000.0,
|
||||
DetourMotionPredictionTimestampSeconds =
|
||||
detourMotionPredictionTimestampSeconds,
|
||||
HasDetourInnovationDiagnostics =
|
||||
hasDetourInnovationDiagnostics,
|
||||
DetourPositionInnovationMeters =
|
||||
detourPositionInnovationMeters,
|
||||
DetourAllowedPositionInnovationMeters =
|
||||
detourAllowedPositionInnovationMeters,
|
||||
DetourHeadingInnovationRadians =
|
||||
detourHeadingInnovationRadians,
|
||||
DetourAllowedHeadingInnovationRadians =
|
||||
detourAllowedHeadingInnovationRadians,
|
||||
DetourLastJumpTriggerReason =
|
||||
detourLastJumpTriggerReason,
|
||||
DetourStateStatus = detourStateStatus,
|
||||
CommandSpeed = commandSpeed,
|
||||
CommandVx = commandVx,
|
||||
CommandVy = commandVy,
|
||||
@@ -703,6 +857,14 @@ namespace MultiWheelC
|
||||
wheelFeedbackFilteredBodyVyMetersPerSecond,
|
||||
WheelFeedbackVelocityEstimateValid =
|
||||
wheelFeedbackVelocityEstimateValid,
|
||||
HasWheelFeedbackAngularVelocityDiagnostics =
|
||||
hasWheelFeedbackAngularVelocityDiagnostics,
|
||||
WheelFeedbackRawBodyOmegaRadiansPerSecond =
|
||||
wheelFeedbackRawBodyOmegaRadiansPerSecond,
|
||||
WheelFeedbackFilteredBodyOmegaRadiansPerSecond =
|
||||
wheelFeedbackFilteredBodyOmegaRadiansPerSecond,
|
||||
WheelFeedbackSampleTimestampSeconds =
|
||||
wheelFeedbackSampleTimestampSeconds,
|
||||
HasGcpCommand = hasGcpCommand,
|
||||
RequestedFrontGcpAngleRadians =
|
||||
requestedFrontGcpAngleRadians,
|
||||
@@ -885,6 +1047,25 @@ namespace MultiWheelC
|
||||
"DetourX," +
|
||||
"DetourY," +
|
||||
"DetourTheta," +
|
||||
"DetourTickRaw," +
|
||||
"DetourLStep," +
|
||||
"HasDetourStateDiagnostics," +
|
||||
"DetourJumpCandidateActive," +
|
||||
"DetourJumpCandidateConsistentFrameCount," +
|
||||
"DetourEstimatedShiftDistanceMeters," +
|
||||
"DetourEstimatedShiftHeadingRadians," +
|
||||
"DetourAutomaticFrameShiftCount," +
|
||||
"DetourStateStatusReason," +
|
||||
"DetourDataAgeMilliseconds," +
|
||||
"DetourSourceFrameIntervalMilliseconds," +
|
||||
"DetourMotionPredictionTimestampSeconds," +
|
||||
"HasDetourInnovationDiagnostics," +
|
||||
"DetourPositionInnovationMeters," +
|
||||
"DetourAllowedPositionInnovationMeters," +
|
||||
"DetourHeadingInnovationRadians," +
|
||||
"DetourAllowedHeadingInnovationRadians," +
|
||||
"DetourLastJumpTriggerReason," +
|
||||
"DetourStateStatus," +
|
||||
"CommandSpeed," +
|
||||
// 保留旧列（deg/s）供历史Python脚本兼容。
|
||||
"CommandAngularSpeed," +
|
||||
@@ -928,6 +1109,10 @@ namespace MultiWheelC
|
||||
"WheelFeedbackRawBodyVyMetersPerSecond," +
|
||||
"WheelFeedbackFilteredBodyVyMetersPerSecond," +
|
||||
"WheelFeedbackVelocityEstimateValid," +
|
||||
"HasWheelFeedbackAngularVelocityDiagnostics," +
|
||||
"WheelFeedbackRawBodyOmegaRadiansPerSecond," +
|
||||
"WheelFeedbackFilteredBodyOmegaRadiansPerSecond," +
|
||||
"WheelFeedbackSampleTimestampSeconds," +
|
||||
"HasSteeringDiagnostics," +
|
||||
"TargetSteerLeftFrontDegrees," +
|
||||
"TargetSteerLeftRearDegrees," +
|
||||
@@ -959,6 +1144,73 @@ namespace MultiWheelC
|
||||
Format(sample.DetourX),
|
||||
Format(sample.DetourY),
|
||||
Format(sample.DetourTheta),
|
||||
sample.DetourTickRaw.ToString(
|
||||
CultureInfo.InvariantCulture),
|
||||
Format(sample.DetourLStep),
|
||||
sample.HasDetourStateDiagnostics
|
||||
? "1"
|
||||
: "0",
|
||||
FormatOptionalBoolean(
|
||||
sample.HasDetourStateDiagnostics,
|
||||
sample.DetourJumpCandidateActive),
|
||||
sample.HasDetourStateDiagnostics
|
||||
? sample
|
||||
.DetourJumpCandidateConsistentFrameCount
|
||||
.ToString(
|
||||
CultureInfo.InvariantCulture)
|
||||
: string.Empty,
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics &&
|
||||
sample.DetourJumpCandidateActive,
|
||||
sample.DetourEstimatedShiftDistanceMeters),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics &&
|
||||
sample.DetourJumpCandidateActive,
|
||||
sample.DetourEstimatedShiftHeadingRadians),
|
||||
sample.HasDetourStateDiagnostics
|
||||
? sample.DetourAutomaticFrameShiftCount
|
||||
.ToString(
|
||||
CultureInfo.InvariantCulture)
|
||||
: string.Empty,
|
||||
sample.HasDetourStateDiagnostics
|
||||
? EscapeCsv(
|
||||
sample.DetourStateStatusReason)
|
||||
: string.Empty,
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics,
|
||||
sample.DetourDataAgeMilliseconds),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics,
|
||||
sample.DetourSourceFrameIntervalMilliseconds),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics,
|
||||
sample.DetourMotionPredictionTimestampSeconds),
|
||||
FormatOptionalBoolean(
|
||||
sample.HasDetourStateDiagnostics,
|
||||
sample.HasDetourInnovationDiagnostics),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics &&
|
||||
sample.HasDetourInnovationDiagnostics,
|
||||
sample.DetourPositionInnovationMeters),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics &&
|
||||
sample.HasDetourInnovationDiagnostics,
|
||||
sample.DetourAllowedPositionInnovationMeters),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics &&
|
||||
sample.HasDetourInnovationDiagnostics,
|
||||
sample.DetourHeadingInnovationRadians),
|
||||
FormatOptional(
|
||||
sample.HasDetourStateDiagnostics &&
|
||||
sample.HasDetourInnovationDiagnostics,
|
||||
sample.DetourAllowedHeadingInnovationRadians),
|
||||
sample.HasDetourStateDiagnostics
|
||||
? EscapeCsv(
|
||||
sample.DetourLastJumpTriggerReason)
|
||||
: string.Empty,
|
||||
sample.HasDetourStateDiagnostics
|
||||
? EscapeCsv(sample.DetourStateStatus)
|
||||
: string.Empty,
|
||||
Format(sample.CommandSpeed),
|
||||
Format(
|
||||
AngleMath.RadiansToDegrees(
|
||||
@@ -1067,6 +1319,21 @@ namespace MultiWheelC
|
||||
? "1"
|
||||
: "0"
|
||||
: string.Empty,
|
||||
FormatOptionalBoolean(
|
||||
sample.HasVelocityDiagnostics,
|
||||
sample.HasWheelFeedbackAngularVelocityDiagnostics),
|
||||
FormatOptional(
|
||||
sample.HasVelocityDiagnostics &&
|
||||
sample.HasWheelFeedbackAngularVelocityDiagnostics,
|
||||
sample.WheelFeedbackRawBodyOmegaRadiansPerSecond),
|
||||
FormatOptional(
|
||||
sample.HasVelocityDiagnostics &&
|
||||
sample.HasWheelFeedbackAngularVelocityDiagnostics,
|
||||
sample.WheelFeedbackFilteredBodyOmegaRadiansPerSecond),
|
||||
FormatOptional(
|
||||
sample.HasVelocityDiagnostics &&
|
||||
sample.HasWheelFeedbackAngularVelocityDiagnostics,
|
||||
sample.WheelFeedbackSampleTimestampSeconds),
|
||||
sample.HasSteeringDiagnostics
|
||||
? "1"
|
||||
: "0",
|
||||
|
||||
Reference in New Issue
Block a user