directional_consistency_filter.py

import numpy as np


class DirectionalConsistencyFilter:
    def __init__(
        self,
        reference_point=None,
    ):
        self.reference_point = reference_point or np.array([0.0, 0.0])
        self.manual_reset(reference_point)
        self.differences = []

    def update(self, new_position: np.ndarray):
        new_position = new_position
        difference_vector = new_position - self.reference_point

        self.gc_differences()

        self.differences.append(difference_vector)

    def consistency_length(self):
        return len(self.differences)

    def gc_differences(self, target_ratio=0.5):
        while len(self.differences) > 0 and (
            np.dot(self.differences[0], self.estimated_direction) < 0
            or self.consistency_ratio < target_ratio
        ):
            self.differences.pop(0)

    @property
    def net_displacement(self):
        return np.sum(self.differences, axis=0)

    @property
    def average_difference(self) -> np.ndarray:
        return np.mean(self.differences, axis=0)

    @property
    def consistency_ratio(self) -> float:
        if not self.differences:
            return 0.0  # Return 0 if no differences have been recorded yet

        sum_diff = self.net_displacement
        sum_vector_magnitude = np.linalg.norm(sum_diff)

        # Compute the sum of magnitudes of individual vectors
        sum_of_magnitudes = np.sum(np.linalg.norm(diff) for diff in self.differences)

        # Compute the consistency ratio
        ratio = sum_vector_magnitude / sum_of_magnitudes if sum_of_magnitudes else 0

        return ratio

    @property
    def estimated_direction(self) -> np.ndarray:
        if not self.differences:
            return np.array([0.0, 0.0])

        average_diff = self.average_difference
        norm = np.linalg.norm(average_diff)
        if norm == 0:
            # To handle the case where average_diff is a zero vector
            return average_diff
        return average_diff / norm