(self,
               robot,
               accelerometer_variance=0.1,
               sensor_variance=0.1,
               initial_variance=0.1,
               moving_window_filter_size=120)
Initiates the velocity estimator.

    See filterpy documentation in the link below for more details.
    https://filterpy.readthedocs.io/en/latest/kalman/KalmanFilter.html

    Args:
      robot: the robot class for velocity estimation.
      accelerometer_variance: noise estimation for accelerometer reading.
      sensor_variance: noise estimation for motor velocity reading.
      initial_covariance: covariance estimation of initial state.
Initiates the velocity estimator.
Initiates the velocity estimator .
def __init__(self,
               robot,
               accelerometer_variance=0.1,
               sensor_variance=0.1,
               initial_variance=0.1,
               moving_window_filter_size=120):
    """Initiates the velocity estimator.

    See filterpy documentation in the link below for more details.
    https://filterpy.readthedocs.io/en/latest/kalman/KalmanFilter.html

    Args:
      robot: the robot class for velocity estimation.
      accelerometer_variance: noise estimation for accelerometer reading.
      sensor_variance: noise estimation for motor velocity reading.
      initial_covariance: covariance estimation of initial state.
    """
    self.robot = robot

    self.filter = KalmanFilter(dim_x=3, dim_z=3, dim_u=3)
    self.filter.x = np.zeros(3)
    self._initial_variance = initial_variance
    self.filter.P = np.eye(3) * self._initial_variance  # State covariance
    self.filter.Q = np.eye(3) * accelerometer_variance
    self.filter.R = np.eye(3) * sensor_variance

    self.filter.H = np.eye(3)  # measurement function (y=H*x)
    self.filter.F = np.eye(3)  # state transition matrix
    self.filter.B = np.eye(3)

    self._window_size = moving_window_filter_size
    self.moving_window_filter_x = MovingWindowFilter(
        window_size=self._window_size)
    self.moving_window_filter_y = MovingWindowFilter(
        window_size=self._window_size)
    self.moving_window_filter_z = MovingWindowFilter(
        window_size=self._window_size)
    self._estimated_velocity = np.zeros(3)
    self._last_timestamp = 0
def __init__ ( self , robot , accelerometer_variance = 0.1 , sensor_variance = 0.1 , initial_variance = 0.1 , moving_window_filter_size = 120 ) : self . robot = robot self . filter = KalmanFilter ( dim_x = 3 , dim_z = 3 , dim_u = 3 ) self . filter . x = np . zeros ( 3 ) self . _initial_variance = initial_variance self . filter . P = np . eye ( 3 ) * self . _initial_variance # State covariance self . filter . Q = np . eye ( 3 ) * accelerometer_variance self . filter . R = np . eye ( 3 ) * sensor_variance self . filter . H = np . eye ( 3 ) # measurement function (y=H*x) self . filter . F = np . eye ( 3 ) # state transition matrix self . filter . B = np . eye ( 3 ) self . _window_size = moving_window_filter_size self . moving_window_filter_x = MovingWindowFilter ( window_size = self . _window_size ) self . moving_window_filter_y = MovingWindowFilter ( window_size = self . _window_size ) self . moving_window_filter_z = MovingWindowFilter ( window_size = self . _window_size ) self . _estimated_velocity = np . zeros ( 3 ) self . _last_timestamp = 0