ParkingRobot/MultiWheelC/Fleet/FleetStateEstimator.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MyParking.Shared;

namespace MultiWheelC.Fleet
{
    // 一辆成员车在主车统一时间轴上的状态样本。
    public readonly struct FleetMemberStateSample
    {
        public FleetMemberStateSample(
            int vehicleId,
            double sampleTimestampSeconds,
            Pose2D poseInWorld,
            Twist2D twistAtVehicleOriginInWorld,
            bool isStateAvailable,
            bool hasValidVelocityEstimate)
        {
            if (vehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(vehicleId),
                    "编队成员车号必须大于零。");
            }

            NumericGuard.EnsureFiniteNonNegative(
                sampleTimestampSeconds,
                nameof(sampleTimestampSeconds));
            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));
            NumericGuard.EnsureFinite(
                twistAtVehicleOriginInWorld,
                nameof(twistAtVehicleOriginInWorld));

            VehicleId = vehicleId;
            SampleTimestampSeconds = sampleTimestampSeconds;
            PoseInWorld = new Pose2D(
                poseInWorld.XMeters,
                poseInWorld.YMeters,
                AngleMath.NormalizeRadians(
                    poseInWorld.YawRadians));
            TwistAtVehicleOriginInWorld =
                twistAtVehicleOriginInWorld;
            IsStateAvailable = isStateAvailable;
            HasValidVelocityEstimate =
                hasValidVelocityEstimate;
        }

        public int VehicleId { get; }

        // 该时间戳必须已经换算到主车/协调器的单调时间轴。
        public double SampleTimestampSeconds { get; }

        public Pose2D PoseInWorld { get; }

        // 成员车体中心处的实际速度，在世界坐标系中表达。
        public Twist2D TwistAtVehicleOriginInWorld { get; }

        public bool IsStateAvailable { get; }

        public bool HasValidVelocityEstimate { get; }
    }

    // 成员实际位姿相对固定布局目标位姿的误差。
    public readonly struct FleetMemberLayoutError
    {
        public FleetMemberLayoutError(
            int vehicleId,
            Pose2D actualPoseInExpectedVehicleFrame)
        {
            if (vehicleId <= 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(vehicleId),
                    "编队成员车号必须大于零。");
            }

            NumericGuard.EnsureFinite(
                actualPoseInExpectedVehicleFrame,
                nameof(actualPoseInExpectedVehicleFrame));

            VehicleId = vehicleId;
            ActualPoseInExpectedVehicleFrame =
                new Pose2D(
                    actualPoseInExpectedVehicleFrame.XMeters,
                    actualPoseInExpectedVehicleFrame.YMeters,
                    AngleMath.NormalizeRadians(
                        actualPoseInExpectedVehicleFrame
                            .YawRadians));
        }

        public int VehicleId { get; }

        // 期望成员车体系中表达的实际成员位姿；理想刚体布局时为Identity。
        public Pose2D ActualPoseInExpectedVehicleFrame { get; }
    }

    // 一次车队状态估计的结果；不可用时不提供FleetState。
    public sealed class FleetStateEstimateResult
    {
        private static readonly IReadOnlyList<FleetMemberLayoutError>
            EmptyMemberErrors = Array.AsReadOnly(
                Array.Empty<FleetMemberLayoutError>());

        private FleetStateEstimateResult(
            bool isAvailable,
            FleetState? state,
            IReadOnlyList<FleetMemberLayoutError> memberErrors,
            string unavailableReason)
        {
            IsAvailable = isAvailable;
            State = state;
            MemberErrors = memberErrors;
            UnavailableReason = unavailableReason;
        }

        public bool IsAvailable { get; }

        public FleetState? State { get; }

        public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }

        public string UnavailableReason { get; }

        internal static FleetStateEstimateResult Available(
            FleetState state,
            FleetMemberLayoutError[] memberErrors)
        {
            return new FleetStateEstimateResult(
                true,
                state,
                Array.AsReadOnly(memberErrors),
                string.Empty);
        }

        internal static FleetStateEstimateResult Unavailable(
            string reason)
        {
            return new FleetStateEstimateResult(
                false,
                null,
                EmptyMemberErrors,
                reason ?? string.Empty);
        }
    }

    // 从各成员状态反算并融合车队虚拟中心状态。
    public sealed class FleetStateEstimator
    {
        private const double TimestampToleranceSeconds = 1e-9;
        private const double MinimumCircularMeanMagnitude = 1e-12;

        private readonly double _maximumMemberStateAgeSeconds;
        private readonly double _maximumPositionDisagreementMeters;
        private readonly double _maximumYawDisagreementRadians;

        public FleetStateEstimator(
            double maximumMemberStateAgeSeconds,
            double maximumPositionDisagreementMeters,
            double maximumYawDisagreementRadians)
        {
            NumericGuard.EnsureFinitePositive(
                maximumMemberStateAgeSeconds,
                nameof(maximumMemberStateAgeSeconds));
            NumericGuard.EnsureFinitePositive(
                maximumPositionDisagreementMeters,
                nameof(maximumPositionDisagreementMeters));
            NumericGuard.EnsureFinitePositive(
                maximumYawDisagreementRadians,
                nameof(maximumYawDisagreementRadians));

            if (maximumYawDisagreementRadians > Math.PI)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(maximumYawDisagreementRadians),
                    "车队候选航向差阈值不能大于π。");
            }

            _maximumMemberStateAgeSeconds =
                maximumMemberStateAgeSeconds;
            _maximumPositionDisagreementMeters =
                maximumPositionDisagreementMeters;
            _maximumYawDisagreementRadians =
                maximumYawDisagreementRadians;
        }

        public FleetStateEstimateResult Estimate(
            FleetLayout layout,
            IReadOnlyList<FleetMemberStateSample> memberStates,
            double targetTimestampSeconds)
        {
            if (layout == null)
            {
                throw new ArgumentNullException(nameof(layout));
            }

            if (memberStates == null)
            {
                throw new ArgumentNullException(nameof(memberStates));
            }

            NumericGuard.EnsureFiniteNonNegative(
                targetTimestampSeconds,
                nameof(targetTimestampSeconds));

            if (memberStates.Count != layout.VehicleCount)
            {
                return FleetStateEstimateResult.Unavailable(
                    $"成员状态数量{memberStates.Count}与布局数量" +
                    $"{layout.VehicleCount}不一致。");
            }

            var statesByVehicleId =
                new Dictionary<int, FleetMemberStateSample>(
                    memberStates.Count);

            for (var index = 0;
                 index < memberStates.Count;
                 index++)
            {
                var memberState = memberStates[index];
                if (memberState.VehicleId <= 0)
                {
                    return FleetStateEstimateResult.Unavailable(
                        $"第{index}个成员状态的车号无效。");
                }

                if (!layout.TryGetVehicle(
                        memberState.VehicleId,
                        out _))
                {
                    return FleetStateEstimateResult.Unavailable(
                        $"成员状态包含布局外车辆" +
                        $"{memberState.VehicleId}。");
                }

                if (statesByVehicleId.ContainsKey(
                        memberState.VehicleId))
                {
                    return FleetStateEstimateResult.Unavailable(
                        $"成员状态包含重复车号" +
                        $"{memberState.VehicleId}。");
                }

                statesByVehicleId.Add(
                    memberState.VehicleId,
                    memberState);
            }

            var alignedMembers =
                new AlignedMemberState[layout.VehicleCount];

            for (var index = 0;
                 index < layout.Vehicles.Count;
                 index++)
            {
                var vehicleLayout = layout.Vehicles[index];
                if (!statesByVehicleId.TryGetValue(
                        vehicleLayout.VehicleId,
                        out var memberState))
                {
                    return FleetStateEstimateResult.Unavailable(
                        $"缺少车辆{vehicleLayout.VehicleId}的状态。");
                }

                var alignmentResult = AlignMemberState(
                    memberState,
                    targetTimestampSeconds,
                    out var alignedPoseInWorld);
                if (alignmentResult != null)
                {
                    return FleetStateEstimateResult.Unavailable(
                        alignmentResult);
                }

                var candidateFleetPoseInWorld =
                    FrameTransform2D.Compose(
                        alignedPoseInWorld,
                        FrameTransform2D.Inverse(
                            vehicleLayout.PoseInFleet));

                alignedMembers[index] =
                    new AlignedMemberState(
                        vehicleLayout,
                        memberState,
                        alignedPoseInWorld,
                        candidateFleetPoseInWorld);
            }

            var disagreementReason =
                FindCandidateDisagreement(alignedMembers);
            if (disagreementReason != null)
            {
                return FleetStateEstimateResult.Unavailable(
                    disagreementReason);
            }

            if (!TryAverageCandidateFleetPose(
                    alignedMembers,
                    out var fleetPoseInWorld))
            {
                return FleetStateEstimateResult.Unavailable(
                    "成员候选航向无法形成唯一的车队平均航向。");
            }

            var hasValidVelocityEstimate =
                TryAverageFleetOriginTwist(
                    alignedMembers,
                    fleetPoseInWorld,
                    out var twistAtFleetOriginInWorld);
            var fleetState = new FleetState(
                targetTimestampSeconds,
                fleetPoseInWorld,
                twistAtFleetOriginInWorld,
                hasValidVelocityEstimate);
            var memberErrors = CalculateMemberErrors(
                alignedMembers,
                fleetPoseInWorld);

            return FleetStateEstimateResult.Available(
                fleetState,
                memberErrors);
        }

        private string AlignMemberState(
            FleetMemberStateSample memberState,
            double targetTimestampSeconds,
            out Pose2D alignedPoseInWorld)
        {
            alignedPoseInWorld = memberState.PoseInWorld;

            if (!memberState.IsStateAvailable)
            {
                return $"车辆{memberState.VehicleId}状态不可用。";
            }

            var ageSeconds =
                targetTimestampSeconds -
                memberState.SampleTimestampSeconds;

            if (ageSeconds < -TimestampToleranceSeconds)
            {
                return $"车辆{memberState.VehicleId}的状态时间晚于" +
                       "本次估计目标时间。";
            }

            if (ageSeconds > _maximumMemberStateAgeSeconds)
            {
                return $"车辆{memberState.VehicleId}的状态已过期：" +
                       $"{ageSeconds:F3}s。";
            }

            if (ageSeconds <= TimestampToleranceSeconds)
            {
                return null;
            }

            if (!memberState.HasValidVelocityEstimate)
            {
                return $"车辆{memberState.VehicleId}缺少时间对齐所需的" +
                       "有效速度。";
            }

            var twist = memberState.TwistAtVehicleOriginInWorld;
            alignedPoseInWorld = new Pose2D(
                memberState.PoseInWorld.XMeters +
                twist.VxMetersPerSecond * ageSeconds,
                memberState.PoseInWorld.YMeters +
                twist.VyMetersPerSecond * ageSeconds,
                AngleMath.NormalizeRadians(
                    memberState.PoseInWorld.YawRadians +
                    twist.OmegaRadiansPerSecond * ageSeconds));
            return null;
        }

        private string FindCandidateDisagreement(
            IReadOnlyList<AlignedMemberState> alignedMembers)
        {
            for (var firstIndex = 0;
                 firstIndex < alignedMembers.Count;
                 firstIndex++)
            {
                var first = alignedMembers[firstIndex];
                for (var secondIndex = firstIndex + 1;
                     secondIndex < alignedMembers.Count;
                     secondIndex++)
                {
                    var second = alignedMembers[secondIndex];
                    var dx =
                        first.CandidateFleetPoseInWorld.XMeters -
                        second.CandidateFleetPoseInWorld.XMeters;
                    var dy =
                        first.CandidateFleetPoseInWorld.YMeters -
                        second.CandidateFleetPoseInWorld.YMeters;
                    var positionDifferenceMeters =
                        Math.Sqrt(dx * dx + dy * dy);
                    var yawDifferenceRadians = Math.Abs(
                        AngleMath.ShortestDifferenceRadians(
                            first.CandidateFleetPoseInWorld
                                .YawRadians,
                            second.CandidateFleetPoseInWorld
                                .YawRadians));

                    if (positionDifferenceMeters >
                        _maximumPositionDisagreementMeters)
                    {
                        return $"车辆{first.VehicleLayout.VehicleId}与" +
                               $"车辆{second.VehicleLayout.VehicleId}反算的" +
                               $"车队中心相差{positionDifferenceMeters:F3}m，" +
                               "超过允许值。";
                    }

                    if (yawDifferenceRadians >
                        _maximumYawDisagreementRadians)
                    {
                        return $"车辆{first.VehicleLayout.VehicleId}与" +
                               $"车辆{second.VehicleLayout.VehicleId}反算的" +
                               $"车队航向相差" +
                               $"{AngleMath.RadiansToDegrees(yawDifferenceRadians):F2}°，" +
                               "超过允许值。";
                    }
                }
            }

            return null;
        }

        private static bool TryAverageCandidateFleetPose(
            IReadOnlyList<AlignedMemberState> alignedMembers,
            out Pose2D fleetPoseInWorld)
        {
            var xMeters = 0.0;
            var yMeters = 0.0;
            var yawCosineSum = 0.0;
            var yawSineSum = 0.0;

            for (var index = 0;
                 index < alignedMembers.Count;
                 index++)
            {
                var candidate =
                    alignedMembers[index]
                        .CandidateFleetPoseInWorld;
                xMeters += candidate.XMeters;
                yMeters += candidate.YMeters;
                yawCosineSum += Math.Cos(candidate.YawRadians);
                yawSineSum += Math.Sin(candidate.YawRadians);
            }

            var count = alignedMembers.Count;
            var circularMeanMagnitude = Math.Sqrt(
                yawCosineSum * yawCosineSum +
                yawSineSum * yawSineSum);
            if (circularMeanMagnitude <
                MinimumCircularMeanMagnitude)
            {
                fleetPoseInWorld = Pose2D.Identity;
                return false;
            }

            fleetPoseInWorld = new Pose2D(
                xMeters / count,
                yMeters / count,
                Math.Atan2(yawSineSum, yawCosineSum));
            return true;
        }

        private static bool TryAverageFleetOriginTwist(
            IReadOnlyList<AlignedMemberState> alignedMembers,
            Pose2D fleetPoseInWorld,
            out Twist2D twistAtFleetOriginInWorld)
        {
            for (var index = 0;
                 index < alignedMembers.Count;
                 index++)
            {
                if (!alignedMembers[index]
                    .MemberState
                    .HasValidVelocityEstimate)
                {
                    twistAtFleetOriginInWorld = Twist2D.Zero;
                    return false;
                }
            }

            var vxMetersPerSecond = 0.0;
            var vyMetersPerSecond = 0.0;
            var omegaRadiansPerSecond = 0.0;

            for (var index = 0;
                 index < alignedMembers.Count;
                 index++)
            {
                var member = alignedMembers[index];
                var twist = member.MemberState
                    .TwistAtVehicleOriginInWorld;
                var memberXFromFleetOrigin =
                    member.AlignedPoseInWorld.XMeters -
                    fleetPoseInWorld.XMeters;
                var memberYFromFleetOrigin =
                    member.AlignedPoseInWorld.YMeters -
                    fleetPoseInWorld.YMeters;

                vxMetersPerSecond +=
                    twist.VxMetersPerSecond +
                    twist.OmegaRadiansPerSecond *
                    memberYFromFleetOrigin;
                vyMetersPerSecond +=
                    twist.VyMetersPerSecond -
                    twist.OmegaRadiansPerSecond *
                    memberXFromFleetOrigin;
                omegaRadiansPerSecond +=
                    twist.OmegaRadiansPerSecond;
            }

            var count = alignedMembers.Count;
            twistAtFleetOriginInWorld = new Twist2D(
                vxMetersPerSecond / count,
                vyMetersPerSecond / count,
                omegaRadiansPerSecond / count);
            return true;
        }

        private static FleetMemberLayoutError[] CalculateMemberErrors(
            IReadOnlyList<AlignedMemberState> alignedMembers,
            Pose2D fleetPoseInWorld)
        {
            var errors =
                new FleetMemberLayoutError[alignedMembers.Count];

            for (var index = 0;
                 index < alignedMembers.Count;
                 index++)
            {
                var member = alignedMembers[index];
                var expectedPoseInWorld =
                    FrameTransform2D.Compose(
                        fleetPoseInWorld,
                        member.VehicleLayout.PoseInFleet);
                var actualPoseInExpectedVehicleFrame =
                    FrameTransform2D.Compose(
                        FrameTransform2D.Inverse(
                            expectedPoseInWorld),
                        member.AlignedPoseInWorld);

                errors[index] = new FleetMemberLayoutError(
                    member.VehicleLayout.VehicleId,
                    actualPoseInExpectedVehicleFrame);
            }

            return errors;
        }

        private readonly struct AlignedMemberState
        {
            public AlignedMemberState(
                VehicleLayout vehicleLayout,
                FleetMemberStateSample memberState,
                Pose2D alignedPoseInWorld,
                Pose2D candidateFleetPoseInWorld)
            {
                VehicleLayout = vehicleLayout;
                MemberState = memberState;
                AlignedPoseInWorld = alignedPoseInWorld;
                CandidateFleetPoseInWorld =
                    candidateFleetPoseInWorld;
            }

            public VehicleLayout VehicleLayout { get; }

            public FleetMemberStateSample MemberState { get; }

            public Pose2D AlignedPoseInWorld { get; }

            public Pose2D CandidateFleetPoseInWorld { get; }
        }
    }
}