Source code for logic.abfilter

import glm

from objects import Tracked




class ABFilter:
    """ The ABFilter class

    Provides methods for using alpha-beta filtering
    """

    def __init__(self, kmax: int = 10):
        """ The ABFilter class initializer.

        :param kmax: Count of maximum points.
        :type kmax: int
        """
        self.t: int = 1  # 1 condition unit
        self.kmax: int = kmax



    def filterAB(self, obj: Tracked, position: glm.vec3, time: int) -> None:
        """ A function that applies an alpha-beta filter to an object.

        :param obj: Tracked object received from the tracker
        :type obj: Tracked
        :param position: Position of tracked object
        :type position: glm.vec3
        :param time: The current time in the simulation
        """
        k = len(obj.trajectory)
        if k == 0:
            obj.trajectory.append(position)
            return
        if k == 1 or k == 2:  # initialization
            obj.filteredVelocityAB = (position - obj.trajectory[-1]) / (
                        time - obj.last_tracked_time)
            obj.trajectory.append(position)

            obj.extrapolatedValueAB = position + (
                        obj.filteredVelocityAB * self.t)
            obj.extrapolatedVelocityAB = obj.filteredVelocityAB
            return

        if k > self.kmax:
            obj.trajectory.pop(0)

        alpha = 2 * (2 * k - 1) / (k * (k + 1))
        beta = 6 / (k * (k + 1))

        obj.trajectory.append(obj.extrapolatedValueAB + (
                alpha * (position - obj.extrapolatedValueAB)))
        obj.filteredVelocityAB = obj.extrapolatedVelocityAB + (
                beta / (time - obj.last_tracked_time) * (
                    position - obj.extrapolatedValueAB))

        obj.extrapolatedValueAB = obj.trajectory[-1] + (
                obj.filteredVelocityAB * (time - obj.last_tracked_time))
        obj.extrapolatedVelocityAB = obj.filteredVelocityAB