implemented xysr filter

boxmot/motion/kalman_filters/xysr_kf.py

@@ -106,7 +106,8 @@ def __init__(self, dim_x, dim_z, dim_u=0, max_obs=50):
 
         self.attr_saved = None
         self.observed = False
-
+
+
     def apply_affine_correction(self, m, t, new_kf):
         """
         Apply to both last state and last observation for OOS smoothing.
@@ -130,28 +131,20 @@ def apply_affine_correction(self, m, t, new_kf):
             scale = np.linalg.norm(m[:, 0])
             self.x[:2] = m @ self.x[:2] + t
             self.x[4:6] = m @ self.x[4:6]
-            # self.x[2] *= scale
-            # self.x[6] *= scale
 
             self.P[:2, :2] = m @ self.P[:2, :2] @ m.T
             self.P[4:6, 4:6] = m @ self.P[4:6, 4:6] @ m.T
-            # self.P[2, 2] *= 2 * scale
-            # self.P[6, 6] *= 2 * scale
 
             # If frozen, also need to update the frozen state for OOS
             if not self.observed and self.attr_saved is not None:
                 self.attr_saved["x"][:2] = m @ self.attr_saved["x"][:2] + t
                 self.attr_saved["x"][4:6] = m @ self.attr_saved["x"][4:6]
-                # self.attr_saved["x"][2] *= scale
-                # self.attr_saved["x"][6] *= scale
 
                 self.attr_saved["P"][:2, :2] = m @ self.attr_saved["P"][:2, :2] @ m.T
                 self.attr_saved["P"][4:6, 4:6] = m @ self.attr_saved["P"][4:6, 4:6] @ m.T
-                # self.attr_saved["P"][2, 2] *= 2 * scale
-                # self.attr_saved["P"][6, 6] *= 2 * scale
 
                 self.attr_saved["last_measurement"][:2] = m @ self.attr_saved["last_measurement"][:2] + t
-                # self.attr_saved["last_measurement"][2] *= scale
+
 
     def predict(self, u=None, B=None, F=None, Q=None):
         """