<commit_before>import numpy as np
import warnings

from RULEngine.Util.Position import Position
from config.config_service import ConfigService

warnings.filterwarnings("ignore", category=np.VisibleDeprecationWarning)


class FriendKalmanFilter:
    def __init__(self):
        cfg = ConfigService()
        self.default_dt = float(cfg.config_dict["GAME"]["ai_timestamp"])
        self.tau_xy = 0.2
        self.tau_orientation = 0.2
        ncameras = int(cfg.config_dict["IMAGE"]["number_of_camera"])

        # Transition model
        self.F = np.array([[1, 0, self.default_dt, 0, 0, 0],  # Position x
                           [0, 1, 0, self.default_dt, 0, 0],  # Position y
                           [0, 0, (1 - self.default_dt/self.tau_xy), 0, 0, 0],  # Speed x
                           [0, 0, 0, (1 - self.default_dt/self.tau_xy), 0, 0],  # Speed y
                           [0, 0, 0, 0, 1, self.default_dt],  # Orientation
                           [0, 0, 0, 0, 0, (1 - self.default_dt/self.tau_orientation)]])  # Speed w
        # Control input model
        self.B = np.array([[0, 0, 0],
                           [0, 0, 0],
                           [self.default_dt/self.tau_xy, 0, 0],  # Speed x
                           [0, self.default_dt/self.tau_xy, 0],  # Speed y
                           [0, 0, 0],
                           [0, 0, self.default_dt/self.tau_orientation]])  # Speed w
        # Observation model
        self.H = [[1, 0, 0, 0, 0, 0] for _ in range(ncameras)]  # Position x
        self.H += [[0, 1, 0, 0, 0, 0] for _ in range(ncameras)]  # Position y
        self.H += [[0, 0, 0, 0, 1, 0] for _ in range(ncameras)]  # Orientation
        self.H = np.array(self.H)
        # Process covariance
        values = np.array([10 ** 2,
                           10 ** 2,
                           10 ** 2,
                           10 ** 2,
                           10 ** (-1),
                           10 ** (-1)]) # Orientation Covariance was 0.01, SB
        self.Q = np.diag(values)
        # Observation covariance
        values = [10 ** (-1) for _ in range(ncameras)]
        values += [10 ** (-1) for _ in range(ncameras)]
        values += [10 ** (-1) for _ in range(ncameras)]
        self.R = np.diag(values)  # Pose * ncameras
        # Initial state covariance
        self.P = 10 ** 3 * np.eye(6)
        self.x = np.array([9999, 9999, 0, 0, 0, 0])

    def predict(self, command):
        if command is None:
            self.x = np.dot(self.F, self.x)
        else:
            conversion_m_to_mm = 1000
            command_xy = conversion_m_to_mm * Position(command[0], command[1]).rotate(self.x[4])
            self.x = np.dot(self.F, self.x) + np.dot(self.B, np.array([command_xy.x, command_xy.y, command[2]]))
        self.P = np.dot(np.dot(self.F, self.P), np.transpose(self.F)) + self.Q

    def update(self, observation):
        obsx = []
        obsy = []<commit_msg>Tuning des matrices de covariances<commit_after>import numpy as np
import warnings

from RULEngine.Util.Position import Position
from config.config_service import ConfigService

warnings.filterwarnings("ignore", category=np.VisibleDeprecationWarning)


class FriendKalmanFilter:
    def __init__(self):
        cfg = ConfigService()
        self.default_dt = float(cfg.config_dict["GAME"]["ai_timestamp"])
        self.tau_xy = 0.2
        self.tau_orientation = 0.2
        ncameras = int(cfg.config_dict["IMAGE"]["number_of_camera"])

        # Transition model
        self.F = np.array([[1, 0, self.default_dt, 0, 0, 0],  # Position x
                           [0, 1, 0, self.default_dt, 0, 0],  # Position y
                           [0, 0, (1 - self.default_dt/self.tau_xy), 0, 0, 0],  # Speed x
                           [0, 0, 0, (1 - self.default_dt/self.tau_xy), 0, 0],  # Speed y
                           [0, 0, 0, 0, 1, self.default_dt],  # Orientation
                           [0, 0, 0, 0, 0, (1 - self.default_dt/self.tau_orientation)]])  # Speed w
        # Control input model
        self.B = np.array([[0, 0, 0],
                           [0, 0, 0],
                           [self.default_dt/self.tau_xy, 0, 0],  # Speed x
                           [0, self.default_dt/self.tau_xy, 0],  # Speed y
                           [0, 0, 0],
                           [0, 0, self.default_dt/self.tau_orientation]])  # Speed w
        # Observation model
        self.H = [[1, 0, 0, 0, 0, 0] for _ in range(ncameras)]  # Position x
        self.H += [[0, 1, 0, 0, 0, 0] for _ in range(ncameras)]  # Position y
        self.H += [[0, 0, 0, 0, 1, 0] for _ in range(ncameras)]  # Orientation
        self.H = np.array(self.H)
        # Process covariance
        values = np.array([10 ** 1,
                           10 ** 1,
                           10 ** 2,
                           10 ** 2,
                           10 ** (-1),
                           10 ** (-1)]) # Orientation Covariance was 0.01, SB
        self.Q = np.diag(values)
        # Observation covariance
        values = [10 ** (1) for _ in range(ncameras)]
        values += [10 ** (1) for _ in range(ncameras)]
        values += [10 ** (-1) for _ in range(ncameras)]
        self.R = np.diag(values)  # Pose * ncameras
        # Initial state covariance
        self.P = 10 ** 3 * np.eye(6)
        self.x = np.array([9999, 9999, 0, 0, 0, 0])

    def predict(self, command):
        if command is None:
            self.x = np.dot(self.F, self.x)
        else:
            conversion_m_to_mm = 1000
            command_xy = conversion_m_to_mm * Position(command[0], command[1]).rotate(self.x[4])
            self.x = np.dot(self.F, self.x) + np.dot(self.B, np.array([command_xy.x, command_xy.y, command[2]]))
        self.P = np.dot(np.dot(self.F, self.P), np.transpose(self.F)) + self.Q

    def update(self, observation):
        obsx = []
        obsy = []
{'fraction_non_alphanumeric': 0.14070181386675706, 'fraction_numerical': 0.04204102390235633, 'mean_word_length': 1.6210573078631718, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2975341', 'n_tokens_mistral': 2128, 'n_tokens_neox': 1927, 'n_words': 492}