Пример #1
0
 public CandidateInfo(DirectedRoad from, double p_from, double p_transition, Emission emission, double p_final)
 {
     From         = from;
     P_From       = p_from;
     P_Transition = p_transition;
     Emission     = emission;
     P_Final      = p_final;
 }
Пример #2
0
        public bool TryUpdateState(Coord coord, DateTime timeStamp = default(DateTime), bool printUpdateAnalyticsToConsole = false)
        {
            var stopwatch = new System.Diagnostics.Stopwatch();

            stopwatch.Start();

            UpdateInfo = new UpdateInfo();
            UpdateInfo.PrevCoordinate    = State.PrevCoord;
            UpdateInfo.Coordinate        = coord;
            UpdateInfo.PrevProbabilities = State.Probabilities;

            // Find nearby roads using search grid
            List <DirectedRoad> nearbyRoads = GetNearbyRoads(coord, Parameters.NearbyRoadsThreshold);

            if (nearbyRoads.Count == 0)
            {
                // If no nearby roads, update fails.
                UpdateInfo.UpdateStatus = Enums.MapMatchUpdateStatus.NoNearbyRoads;
                stopwatch.Stop();
                UpdateInfo.UpdateTimeInMilliseconds = stopwatch.ElapsedMilliseconds;
                if (printUpdateAnalyticsToConsole)
                {
                    UpdateInfo.PrintUpdateSummary();
                }

                return(false);
            }

            // Project coordinate onto roads
            List <ProjectToRoad> nearbyRoadProjections = nearbyRoads.Select(x => new ProjectToRoad(coord, x)).ToList();
            // Initialize new transition memory
            var transitionMemory = new Dictionary <DirectedRoad, DirectedRoad>();
            // Initialize new probability vector
            var newProbabilityVector = new ProbabilityVector <DirectedRoad>();

            if (State.PrevCoord == null)
            {
                foreach (var projection in nearbyRoadProjections)
                {
                    // Get emission
                    Emission emission = MarkovProbabilityHelpers.EmissionProbability(projection, Parameters.Sigma);

                    // Log info
                    var candidateInfo = new CandidateInfo();
                    candidateInfo.Emission = emission;
                    UpdateInfo.CandidateDetails[projection.Road] = candidateInfo;
                    UpdateInfo.Emissions[projection.Road]        = emission;

                    // Update probability and transition memory
                    newProbabilityVector[projection.Road] = emission.Probability;
                    transitionMemory[projection.Road]     = null;
                }
            }
            else
            {
                foreach (var projection in nearbyRoadProjections)
                {
                    //Calculate emission probability
                    Emission emission = MarkovProbabilityHelpers.EmissionProbability(projection, Parameters.Sigma);
                    UpdateInfo.Emissions[projection.Road] = emission;

                    //Calculate maximum transition from possible prev state
                    var maxCandidates = new Dictionary <Transition, double>();
                    UpdateInfo.AllTransitions[projection.Road] = new List <Transition>();
                    foreach (var prevProjection in State.PrevNearbyRoadsAndProjections)
                    {
                        Transition transition = MarkovProbabilityHelpers.TransitionProbability(Graph, prevProjection, projection, Parameters);
                        UpdateInfo.AllTransitions[projection.Road].Add(transition);
                        maxCandidates[transition] = transition.Probability * State.Probabilities[prevProjection.Road];
                    }
                    var maxPair = maxCandidates.Aggregate((x, y) => x.Value > y.Value ? x : y);

                    // Log info
                    UpdateInfo.Transitions[projection.Road] = maxPair.Value;
                    var candidateInfo = new CandidateInfo();
                    candidateInfo.Emission     = emission;
                    candidateInfo.From         = maxPair.Key.From;
                    candidateInfo.P_From       = State.Probabilities[maxPair.Key.From];
                    candidateInfo.P_Transition = maxPair.Key.Probability;
                    candidateInfo.P_Final      = maxPair.Value * emission.Probability;
                    UpdateInfo.CandidateDetails[projection.Road] = candidateInfo;

                    // Update probability and transition memory
                    newProbabilityVector[projection.Road] = maxPair.Value * emission.Probability;
                    transitionMemory[projection.Road]     = maxPair.Key.From;
                }
            }

            UpdateInfo.NonNormalizedProbabilityVector = newProbabilityVector;
            UpdateInfo.Probabilities = newProbabilityVector.Normalize();

            // CHECK IF UPDATE FAILED:
            // Emission probabilities were practically zero (i.e. no nearby roads)
            if (UpdateInfo.Emissions.Values.Select(x => x.Probability).Sum() < double.Epsilon)
            {
                UpdateInfo.UpdateStatus = Enums.MapMatchUpdateStatus.ZeroEmissions;
                stopwatch.Stop();
                UpdateInfo.UpdateTimeInMilliseconds = stopwatch.ElapsedMilliseconds;
                if (printUpdateAnalyticsToConsole)
                {
                    UpdateInfo.PrintUpdateSummary();
                }
                return(false);
            }
            // No transitions were possible
            if (UpdateInfo.Transitions.Count > 0 && UpdateInfo.Transitions.Values.Sum() < double.Epsilon)
            {
                UpdateInfo.UpdateStatus = Enums.MapMatchUpdateStatus.NoPossibleTransitions;
                stopwatch.Stop();
                UpdateInfo.UpdateTimeInMilliseconds = stopwatch.ElapsedMilliseconds;
                if (printUpdateAnalyticsToConsole)
                {
                    UpdateInfo.PrintUpdateSummary();
                }
                return(false);
            }
            if (newProbabilityVector.GetSum() < double.Epsilon)
            {
                UpdateInfo.UpdateStatus = Enums.MapMatchUpdateStatus.CrossoverProblem;
                stopwatch.Stop();
                UpdateInfo.UpdateTimeInMilliseconds = stopwatch.ElapsedMilliseconds;
                if (printUpdateAnalyticsToConsole)
                {
                    UpdateInfo.PrintUpdateSummary();
                }
                return(false);
            }

            //UPDATE STATE:
            newProbabilityVector = newProbabilityVector.Normalize();

            // 1. Update probability vector
            State.Probabilities = newProbabilityVector;

            //2. Add to transition memory
            State.TransitionMemory.Add(transitionMemory);

            //3. Update prev coord and date
            State.PrevCoord = coord;
            State.PrevTime  = timeStamp;

            // 4.
            State.PrevNearbyRoadsAndProjections = nearbyRoadProjections;

            //LastUpdateAnalytics.ProbabilityVector = State.Probabilities;
            UpdateInfo.UpdateStatus = Enums.MapMatchUpdateStatus.SuccessfullyUpdated;
            stopwatch.Stop();
            UpdateInfo.UpdateTimeInMilliseconds = stopwatch.Elapsed.TotalMilliseconds;
            //Console.WriteLine(analytics.BuildSummaryString(5));

            if (printUpdateAnalyticsToConsole)
            {
                UpdateInfo.PrintUpdateSummary(20);
            }
            return(true);
        }