669 lines
27 KiB
C#
669 lines
27 KiB
C#
using System;
|
||
using CommonUsage.Chassis;
|
||
using MyParking.Shared;
|
||
using System.Diagnostics;
|
||
|
||
namespace MultiWheelC.StateEstimation
|
||
{
|
||
/// <summary>
|
||
/// 保留外部状态源的Detour位姿，以轮组反馈替换平面线速度，并向短时位姿预测提供角速度。
|
||
/// </summary>
|
||
public sealed class WheelFeedbackVehicleStateProvider
|
||
: IVehicleStateProvider
|
||
{
|
||
private readonly Stopwatch _wheelSpeedClock = Stopwatch.StartNew();
|
||
public const double DefaultVelocityFilterTimeConstantSeconds =
|
||
0.10;
|
||
|
||
private readonly object _syncRoot = new object();
|
||
private readonly IVehicleStateProvider _poseProvider;
|
||
private readonly MultiWheelChassis _chassis;
|
||
private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
|
||
private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
|
||
private readonly FirstOrderLowPassFilter _angularSpeedFilter;
|
||
|
||
private bool _hasPreviousTimestamp;
|
||
private double _previousTimestampSeconds;
|
||
private bool _hasVelocityDiagnostics;
|
||
private double _latestDetourBodyVxMetersPerSecond;
|
||
private bool _latestDetourVelocityValid;
|
||
private double _latestRawWheelBodyVxMetersPerSecond;
|
||
private double _latestFilteredWheelBodyVxMetersPerSecond;
|
||
private double _latestRawWheelBodyVyMetersPerSecond;
|
||
private double _latestFilteredWheelBodyVyMetersPerSecond;
|
||
private double _latestRawWheelBodyOmegaRadiansPerSecond;
|
||
private double _latestFilteredWheelBodyOmegaRadiansPerSecond;
|
||
private double _latestWheelSampleTimestampSeconds;
|
||
private bool _latestWheelVelocityValid;
|
||
private bool _latestWheelFeedbackReadSucceeded;
|
||
|
||
/// <summary>
|
||
/// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
|
||
/// </summary>
|
||
public WheelFeedbackVehicleStateProvider(
|
||
IVehicleStateProvider poseProvider,
|
||
MultiWheelChassis chassis)
|
||
: this(
|
||
poseProvider,
|
||
chassis,
|
||
DefaultVelocityFilterTimeConstantSeconds)
|
||
{
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建使用指定低通时间常数的电机反馈平面速度状态源。
|
||
/// </summary>
|
||
public WheelFeedbackVehicleStateProvider(
|
||
IVehicleStateProvider poseProvider,
|
||
MultiWheelChassis chassis,
|
||
double velocityFilterTimeConstantSeconds)
|
||
{
|
||
_poseProvider = poseProvider ??
|
||
throw new ArgumentNullException(
|
||
nameof(poseProvider));
|
||
_chassis = chassis ??
|
||
throw new ArgumentNullException(
|
||
nameof(chassis));
|
||
_longitudinalSpeedFilter =
|
||
new FirstOrderLowPassFilter(
|
||
velocityFilterTimeConstantSeconds);
|
||
_lateralSpeedFilter =
|
||
new FirstOrderLowPassFilter(
|
||
velocityFilterTimeConstantSeconds);
|
||
_angularSpeedFilter =
|
||
new FirstOrderLowPassFilter(
|
||
velocityFilterTimeConstantSeconds);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取最近一次读取失败的原因，正常时为空字符串。
|
||
/// </summary>
|
||
public string LastFailureReason { get; private set; } =
|
||
string.Empty;
|
||
|
||
/// <summary>
|
||
/// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
|
||
/// </summary>
|
||
public string LastHeadingFailureReason { get; private set; } =
|
||
string.Empty;
|
||
|
||
/// <summary>
|
||
/// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
|
||
/// </summary>
|
||
public bool TryGetState(out VehicleState state)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
try
|
||
{
|
||
ReadFilteredWheelTwist(
|
||
out var filteredWheelTwist,
|
||
out _,
|
||
out var hasValidWheelSpeedEstimate);
|
||
|
||
// Detour位姿跳变确认期间需要用轮速维持短时运动预测。
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider)
|
||
{
|
||
detourStateProvider.UpdateWheelVelocityEstimate(
|
||
filteredWheelTwist.VxMetersPerSecond,
|
||
filteredWheelTwist.VyMetersPerSecond,
|
||
filteredWheelTwist.OmegaRadiansPerSecond,
|
||
hasValidWheelSpeedEstimate);
|
||
}
|
||
|
||
if (!_poseProvider.TryGetState(
|
||
out var poseState))
|
||
{
|
||
state = default;
|
||
LastFailureReason =
|
||
GetPoseProviderFailureReason();
|
||
return false;
|
||
}
|
||
|
||
_latestDetourBodyVxMetersPerSecond =
|
||
poseState.TwistInBody.VxMetersPerSecond;
|
||
_latestDetourVelocityValid =
|
||
poseState.HasValidVelocityEstimate;
|
||
_hasVelocityDiagnostics = true;
|
||
|
||
// 轮组反馈有效后统一使用滤波后的平面速度；初始化期间
|
||
// 暂时保留Detour角速度作为回退值。
|
||
var omegaRadiansPerSecond =
|
||
hasValidWheelSpeedEstimate
|
||
? filteredWheelTwist
|
||
.OmegaRadiansPerSecond
|
||
: poseState.TwistInBody
|
||
.OmegaRadiansPerSecond;
|
||
var twistInBody = new Twist2D(
|
||
filteredWheelTwist.VxMetersPerSecond,
|
||
filteredWheelTwist.VyMetersPerSecond,
|
||
omegaRadiansPerSecond);
|
||
|
||
var twistInWorld =
|
||
FrameTransform2D
|
||
.TransformTwistAtSamePoint(
|
||
poseState.PoseInWorld,
|
||
twistInBody);
|
||
|
||
state = new VehicleState(
|
||
poseState.SampleTimestampSeconds,
|
||
poseState.PoseInWorld,
|
||
twistInWorld,
|
||
hasValidWheelSpeedEstimate);
|
||
|
||
LastFailureReason = string.Empty;
|
||
return true;
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
_latestWheelFeedbackReadSucceeded = false;
|
||
state = default;
|
||
LastFailureReason =
|
||
"舵轮电机反馈车体速度解算失败：" +
|
||
exception.Message;
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取并滤波轮组反馈速度，不访问Detour；首帧仅建立滤波时间基准并返回false。
|
||
/// </summary>
|
||
public bool TryGetWheelTwist(
|
||
out Twist2D twistInBody,
|
||
out double sampleTimestampSeconds)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
try
|
||
{
|
||
ReadFilteredWheelTwist(
|
||
out var filteredWheelTwist,
|
||
out sampleTimestampSeconds,
|
||
out var hasValidWheelSpeedEstimate);
|
||
|
||
if (!hasValidWheelSpeedEstimate)
|
||
{
|
||
twistInBody = Twist2D.Zero;
|
||
LastFailureReason =
|
||
"轮组速度估计正在建立采样时间基准。";
|
||
return false;
|
||
}
|
||
|
||
twistInBody = filteredWheelTwist;
|
||
LastFailureReason = string.Empty;
|
||
return true;
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
_latestWheelFeedbackReadSucceeded = false;
|
||
twistInBody = Twist2D.Zero;
|
||
sampleTimestampSeconds = 0.0;
|
||
LastFailureReason =
|
||
"舵轮电机反馈车体速度解算失败：" +
|
||
exception.Message;
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
|
||
/// </summary>
|
||
public bool TryGetHeadingRadians(
|
||
out double headingRadians)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
TryGetState(out _);
|
||
|
||
if (!_latestWheelFeedbackReadSucceeded)
|
||
{
|
||
headingRadians = 0.0;
|
||
LastHeadingFailureReason =
|
||
string.IsNullOrWhiteSpace(
|
||
LastFailureReason)
|
||
? "舵轮反馈当前不可用，无法校验航向。"
|
||
: LastFailureReason;
|
||
return false;
|
||
}
|
||
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider)
|
||
{
|
||
var success = detourStateProvider
|
||
.TryGetLatestReliableHeadingRadians(
|
||
out headingRadians);
|
||
LastHeadingFailureReason = success
|
||
? string.Empty
|
||
: detourStateProvider
|
||
.LastHeadingFailureReason;
|
||
return success;
|
||
}
|
||
|
||
if (_poseProvider.TryGetState(
|
||
out var poseState))
|
||
{
|
||
headingRadians =
|
||
poseState.PoseInWorld.YawRadians;
|
||
LastHeadingFailureReason = string.Empty;
|
||
return true;
|
||
}
|
||
|
||
headingRadians = 0.0;
|
||
LastHeadingFailureReason =
|
||
GetPoseProviderFailureReason();
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 原地自转停车后，允许基础Detour状态源重新确认有限位置偏移。
|
||
/// </summary>
|
||
public void BeginPostRotationPositionRecovery()
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider)
|
||
{
|
||
detourStateProvider
|
||
.BeginPostRotationPositionRecovery();
|
||
}
|
||
}
|
||
}
|
||
|
||
private string GetPoseProviderFailureReason()
|
||
{
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider &&
|
||
!string.IsNullOrWhiteSpace(
|
||
detourStateProvider.LastFailureReason))
|
||
{
|
||
return detourStateProvider.LastFailureReason;
|
||
}
|
||
|
||
return "基础位姿状态源暂时不可用。";
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
|
||
/// </summary>
|
||
public bool TryGetLatestVelocityDiagnostics(
|
||
out double detourBodyVxMetersPerSecond,
|
||
out bool detourVelocityValid,
|
||
out double rawWheelBodyVxMetersPerSecond,
|
||
out double filteredWheelBodyVxMetersPerSecond,
|
||
out double rawWheelBodyVyMetersPerSecond,
|
||
out double filteredWheelBodyVyMetersPerSecond,
|
||
out bool wheelVelocityValid)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
detourBodyVxMetersPerSecond =
|
||
_latestDetourBodyVxMetersPerSecond;
|
||
detourVelocityValid =
|
||
_latestDetourVelocityValid;
|
||
rawWheelBodyVxMetersPerSecond =
|
||
_latestRawWheelBodyVxMetersPerSecond;
|
||
filteredWheelBodyVxMetersPerSecond =
|
||
_latestFilteredWheelBodyVxMetersPerSecond;
|
||
rawWheelBodyVyMetersPerSecond =
|
||
_latestRawWheelBodyVyMetersPerSecond;
|
||
filteredWheelBodyVyMetersPerSecond =
|
||
_latestFilteredWheelBodyVyMetersPerSecond;
|
||
wheelVelocityValid =
|
||
_latestWheelVelocityValid;
|
||
return _hasVelocityDiagnostics;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取最近一帧Detour纵向速度及轮组原始/滤波Vx、Vy、Vw和采样时间。
|
||
/// </summary>
|
||
public bool TryGetLatestVelocityDiagnostics(
|
||
out double detourBodyVxMetersPerSecond,
|
||
out bool detourVelocityValid,
|
||
out double rawWheelBodyVxMetersPerSecond,
|
||
out double filteredWheelBodyVxMetersPerSecond,
|
||
out double rawWheelBodyVyMetersPerSecond,
|
||
out double filteredWheelBodyVyMetersPerSecond,
|
||
out double rawWheelBodyOmegaRadiansPerSecond,
|
||
out double filteredWheelBodyOmegaRadiansPerSecond,
|
||
out double wheelSampleTimestampSeconds,
|
||
out bool wheelVelocityValid)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
detourBodyVxMetersPerSecond =
|
||
_latestDetourBodyVxMetersPerSecond;
|
||
detourVelocityValid =
|
||
_latestDetourVelocityValid;
|
||
rawWheelBodyVxMetersPerSecond =
|
||
_latestRawWheelBodyVxMetersPerSecond;
|
||
filteredWheelBodyVxMetersPerSecond =
|
||
_latestFilteredWheelBodyVxMetersPerSecond;
|
||
rawWheelBodyVyMetersPerSecond =
|
||
_latestRawWheelBodyVyMetersPerSecond;
|
||
filteredWheelBodyVyMetersPerSecond =
|
||
_latestFilteredWheelBodyVyMetersPerSecond;
|
||
rawWheelBodyOmegaRadiansPerSecond =
|
||
_latestRawWheelBodyOmegaRadiansPerSecond;
|
||
filteredWheelBodyOmegaRadiansPerSecond =
|
||
_latestFilteredWheelBodyOmegaRadiansPerSecond;
|
||
wheelSampleTimestampSeconds =
|
||
_latestWheelSampleTimestampSeconds;
|
||
wheelVelocityValid =
|
||
_latestWheelVelocityValid;
|
||
return _hasVelocityDiagnostics;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取Detour跳变候选、自动坐标连续化和数据新鲜度诊断。
|
||
/// </summary>
|
||
public bool TryGetLatestDetourDiagnostics(
|
||
out bool jumpCandidateActive,
|
||
out int jumpCandidateConsistentFrameCount,
|
||
out double estimatedShiftDistanceMeters,
|
||
out double estimatedShiftHeadingRadians,
|
||
out int automaticFrameShiftCount,
|
||
out string stateStatusReason,
|
||
out double detourDataAgeMilliseconds)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider)
|
||
{
|
||
var hasDiagnostics = detourStateProvider
|
||
.TryGetLatestDiagnostics(
|
||
out jumpCandidateActive,
|
||
out jumpCandidateConsistentFrameCount,
|
||
out estimatedShiftDistanceMeters,
|
||
out estimatedShiftHeadingRadians,
|
||
out automaticFrameShiftCount,
|
||
out stateStatusReason,
|
||
out detourDataAgeMilliseconds);
|
||
|
||
if (string.IsNullOrWhiteSpace(
|
||
stateStatusReason) &&
|
||
!string.IsNullOrWhiteSpace(
|
||
LastFailureReason))
|
||
{
|
||
stateStatusReason = LastFailureReason;
|
||
}
|
||
|
||
return hasDiagnostics;
|
||
}
|
||
|
||
jumpCandidateActive = false;
|
||
jumpCandidateConsistentFrameCount = 0;
|
||
estimatedShiftDistanceMeters = 0.0;
|
||
estimatedShiftHeadingRadians = 0.0;
|
||
automaticFrameShiftCount = 0;
|
||
stateStatusReason = LastFailureReason;
|
||
detourDataAgeMilliseconds = 0.0;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
|
||
/// </summary>
|
||
public bool TryGetLatestDetourDiagnostics(
|
||
out bool jumpCandidateActive,
|
||
out int jumpCandidateConsistentFrameCount,
|
||
out double estimatedShiftDistanceMeters,
|
||
out double estimatedShiftHeadingRadians,
|
||
out int automaticFrameShiftCount,
|
||
out double sourceFrameIntervalSeconds,
|
||
out double motionPredictionTimestampSeconds,
|
||
out bool hasInnovationDiagnostics,
|
||
out double positionInnovationMeters,
|
||
out double allowedPositionInnovationMeters,
|
||
out double headingInnovationRadians,
|
||
out double allowedHeadingInnovationRadians,
|
||
out string jumpCandidateTriggerReason,
|
||
out string stateStatus,
|
||
out string stateStatusReason,
|
||
out double detourDataAgeMilliseconds)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider)
|
||
{
|
||
var hasDiagnostics = detourStateProvider
|
||
.TryGetLatestDiagnostics(
|
||
out jumpCandidateActive,
|
||
out jumpCandidateConsistentFrameCount,
|
||
out estimatedShiftDistanceMeters,
|
||
out estimatedShiftHeadingRadians,
|
||
out automaticFrameShiftCount,
|
||
out sourceFrameIntervalSeconds,
|
||
out motionPredictionTimestampSeconds,
|
||
out hasInnovationDiagnostics,
|
||
out positionInnovationMeters,
|
||
out allowedPositionInnovationMeters,
|
||
out headingInnovationRadians,
|
||
out allowedHeadingInnovationRadians,
|
||
out jumpCandidateTriggerReason,
|
||
out stateStatus,
|
||
out stateStatusReason,
|
||
out detourDataAgeMilliseconds);
|
||
|
||
if (string.IsNullOrWhiteSpace(
|
||
stateStatusReason) &&
|
||
!string.IsNullOrWhiteSpace(
|
||
LastFailureReason))
|
||
{
|
||
stateStatus = "Unavailable";
|
||
stateStatusReason = LastFailureReason;
|
||
}
|
||
|
||
return hasDiagnostics;
|
||
}
|
||
|
||
jumpCandidateActive = false;
|
||
jumpCandidateConsistentFrameCount = 0;
|
||
estimatedShiftDistanceMeters = 0.0;
|
||
estimatedShiftHeadingRadians = 0.0;
|
||
automaticFrameShiftCount = 0;
|
||
sourceFrameIntervalSeconds = 0.0;
|
||
motionPredictionTimestampSeconds = 0.0;
|
||
hasInnovationDiagnostics = false;
|
||
positionInnovationMeters = 0.0;
|
||
allowedPositionInnovationMeters = 0.0;
|
||
headingInnovationRadians = 0.0;
|
||
allowedHeadingInnovationRadians = 0.0;
|
||
jumpCandidateTriggerReason = string.Empty;
|
||
stateStatus = "Unavailable";
|
||
stateStatusReason = LastFailureReason;
|
||
detourDataAgeMilliseconds = 0.0;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除基础位姿状态、坐标连续化状态以及电机反馈速度滤波历史。
|
||
/// </summary>
|
||
public void Reset()
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
if (_poseProvider is DetourVehicleStateProvider
|
||
detourStateProvider)
|
||
{
|
||
detourStateProvider.Reset();
|
||
}
|
||
|
||
_longitudinalSpeedFilter.Reset();
|
||
_lateralSpeedFilter.Reset();
|
||
_angularSpeedFilter.Reset();
|
||
_wheelSpeedClock.Restart();
|
||
_hasPreviousTimestamp = false;
|
||
_previousTimestampSeconds = 0.0;
|
||
_hasVelocityDiagnostics = false;
|
||
_latestDetourBodyVxMetersPerSecond = 0.0;
|
||
_latestDetourVelocityValid = false;
|
||
_latestRawWheelBodyVxMetersPerSecond = 0.0;
|
||
_latestFilteredWheelBodyVxMetersPerSecond = 0.0;
|
||
_latestRawWheelBodyVyMetersPerSecond = 0.0;
|
||
_latestFilteredWheelBodyVyMetersPerSecond = 0.0;
|
||
_latestRawWheelBodyOmegaRadiansPerSecond = 0.0;
|
||
_latestFilteredWheelBodyOmegaRadiansPerSecond = 0.0;
|
||
_latestWheelSampleTimestampSeconds = 0.0;
|
||
_latestWheelVelocityValid = false;
|
||
_latestWheelFeedbackReadSucceeded = false;
|
||
LastFailureReason = string.Empty;
|
||
LastHeadingFailureReason = string.Empty;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从底盘读取一次轮组速度，统一转换为SI单位并更新共用低通滤波状态。
|
||
/// </summary>
|
||
private void ReadFilteredWheelTwist(
|
||
out Twist2D filteredTwistInBody,
|
||
out double sampleTimestampSeconds,
|
||
out bool hasValidWheelSpeedEstimate)
|
||
{
|
||
var actualCarSpeed =
|
||
_chassis.GetCarSpeed(true);
|
||
var rawBodyVxMetersPerSecond =
|
||
(double)actualCarSpeed.Vx;
|
||
var rawBodyVyMetersPerSecond =
|
||
(double)actualCarSpeed.Vy;
|
||
// CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
|
||
// 状态估计内部统一转换为rad/s。
|
||
var rawBodyOmegaRadiansPerSecond =
|
||
AngleMath.DegreesToRadians(
|
||
actualCarSpeed.Vw);
|
||
|
||
NumericGuard.EnsureFinite(
|
||
rawBodyVxMetersPerSecond,
|
||
"电机反馈车体纵向速度");
|
||
NumericGuard.EnsureFinite(
|
||
rawBodyVyMetersPerSecond,
|
||
"电机反馈车体横向速度");
|
||
NumericGuard.EnsureFinite(
|
||
rawBodyOmegaRadiansPerSecond,
|
||
"电机反馈车体角速度");
|
||
|
||
sampleTimestampSeconds =
|
||
_wheelSpeedClock.Elapsed.TotalSeconds;
|
||
|
||
UpdateBodyVelocityFilters(
|
||
rawBodyVxMetersPerSecond,
|
||
rawBodyVyMetersPerSecond,
|
||
rawBodyOmegaRadiansPerSecond,
|
||
sampleTimestampSeconds,
|
||
out var filteredBodyVxMetersPerSecond,
|
||
out var filteredBodyVyMetersPerSecond,
|
||
out var filteredBodyOmegaRadiansPerSecond,
|
||
out hasValidWheelSpeedEstimate);
|
||
|
||
filteredTwistInBody = new Twist2D(
|
||
filteredBodyVxMetersPerSecond,
|
||
filteredBodyVyMetersPerSecond,
|
||
filteredBodyOmegaRadiansPerSecond);
|
||
|
||
_latestRawWheelBodyVxMetersPerSecond =
|
||
rawBodyVxMetersPerSecond;
|
||
_latestFilteredWheelBodyVxMetersPerSecond =
|
||
filteredBodyVxMetersPerSecond;
|
||
_latestRawWheelBodyVyMetersPerSecond =
|
||
rawBodyVyMetersPerSecond;
|
||
_latestFilteredWheelBodyVyMetersPerSecond =
|
||
filteredBodyVyMetersPerSecond;
|
||
_latestRawWheelBodyOmegaRadiansPerSecond =
|
||
rawBodyOmegaRadiansPerSecond;
|
||
_latestFilteredWheelBodyOmegaRadiansPerSecond =
|
||
filteredBodyOmegaRadiansPerSecond;
|
||
_latestWheelSampleTimestampSeconds =
|
||
sampleTimestampSeconds;
|
||
_latestWheelVelocityValid =
|
||
hasValidWheelSpeedEstimate;
|
||
_latestWheelFeedbackReadSucceeded = true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
|
||
/// </summary>
|
||
private void UpdateBodyVelocityFilters(
|
||
double rawBodyVxMetersPerSecond,
|
||
double rawBodyVyMetersPerSecond,
|
||
double rawBodyOmegaRadiansPerSecond,
|
||
double timestampSeconds,
|
||
out double filteredBodyVxMetersPerSecond,
|
||
out double filteredBodyVyMetersPerSecond,
|
||
out double filteredBodyOmegaRadiansPerSecond,
|
||
out bool hasValidWheelSpeedEstimate)
|
||
{
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
timestampSeconds,
|
||
nameof(timestampSeconds));
|
||
|
||
if (!_hasPreviousTimestamp)
|
||
{
|
||
_longitudinalSpeedFilter.Reset(
|
||
rawBodyVxMetersPerSecond);
|
||
_lateralSpeedFilter.Reset(
|
||
rawBodyVyMetersPerSecond);
|
||
_angularSpeedFilter.Reset(
|
||
rawBodyOmegaRadiansPerSecond);
|
||
_previousTimestampSeconds = timestampSeconds;
|
||
_hasPreviousTimestamp = true;
|
||
hasValidWheelSpeedEstimate = false;
|
||
filteredBodyVxMetersPerSecond =
|
||
rawBodyVxMetersPerSecond;
|
||
filteredBodyVyMetersPerSecond =
|
||
rawBodyVyMetersPerSecond;
|
||
filteredBodyOmegaRadiansPerSecond =
|
||
rawBodyOmegaRadiansPerSecond;
|
||
return;
|
||
}
|
||
|
||
var deltaTimeSeconds =
|
||
timestampSeconds -
|
||
_previousTimestampSeconds;
|
||
_previousTimestampSeconds = timestampSeconds;
|
||
|
||
if (deltaTimeSeconds <= 0.0)
|
||
{
|
||
_longitudinalSpeedFilter.Reset(
|
||
rawBodyVxMetersPerSecond);
|
||
_lateralSpeedFilter.Reset(
|
||
rawBodyVyMetersPerSecond);
|
||
_angularSpeedFilter.Reset(
|
||
rawBodyOmegaRadiansPerSecond);
|
||
hasValidWheelSpeedEstimate = false;
|
||
filteredBodyVxMetersPerSecond =
|
||
rawBodyVxMetersPerSecond;
|
||
filteredBodyVyMetersPerSecond =
|
||
rawBodyVyMetersPerSecond;
|
||
filteredBodyOmegaRadiansPerSecond =
|
||
rawBodyOmegaRadiansPerSecond;
|
||
return;
|
||
}
|
||
|
||
hasValidWheelSpeedEstimate = true;
|
||
filteredBodyVxMetersPerSecond =
|
||
_longitudinalSpeedFilter.Update(
|
||
rawBodyVxMetersPerSecond,
|
||
deltaTimeSeconds);
|
||
filteredBodyVyMetersPerSecond =
|
||
_lateralSpeedFilter.Update(
|
||
rawBodyVyMetersPerSecond,
|
||
deltaTimeSeconds);
|
||
filteredBodyOmegaRadiansPerSecond =
|
||
_angularSpeedFilter.Update(
|
||
rawBodyOmegaRadiansPerSecond,
|
||
deltaTimeSeconds);
|
||
}
|
||
|
||
}
|
||
}
|