Formatted Q matrix in obstacle class

Changed global transform and infering orientation from velocity flags to true

Author: alexanderjyuen

kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py

@@ -49,8 +49,8 @@ def __init__(self):
                 ("vel_filter", [0.1, 2.0]),
                 ("height_filter", [-2.0, 2.0]),
                 ("cost_filter", 1.0),
-                ("transform_to_global_frame", False),
-                ("infer_orientation_from_velocity", False),
+                ("transform_to_global_frame", True),
+                ("infer_orientation_from_velocity", True),
             ],
         )
         self.global_frame = self.get_parameter("global_frame")._value

kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py

@@ -103,58 +103,13 @@ def predict(self, dt):
         dt2 = dt**2
         dt3 = dt * dt2
         dt4 = dt2**2
-        Q = np.array(
-            [
-                [
-                    dt4 * self.process_noise_cov[0] / 4,
-                    0,
-                    0,
-                    dt3 * self.process_noise_cov[0] / 2,
-                    0,
-                    0,
-                ],
-                [
-                    0,
-                    dt4 * self.process_noise_cov[1] / 4,
-                    0,
-                    0,
-                    dt3 * self.process_noise_cov[1] / 2,
-                    0,
-                ],
-                [
-                    0,
-                    0,
-                    dt4 * self.process_noise_cov[2] / 4,
-                    0,
-                    0,
-                    dt3 * self.process_noise_cov[2] / 2,
-                ],
-                [
-                    dt3 * self.process_noise_cov[0] / 2,
-                    0,
-                    0,
-                    dt2 * self.process_noise_cov[0],
-                    0,
-                    0,
-                ],
-                [
-                    0,
-                    dt3 * self.process_noise_cov[1] / 2,
-                    0,
-                    0,
-                    dt2 * self.process_noise_cov[1],
-                    0,
-                ],
-                [
-                    0,
-                    0,
-                    dt3 * self.process_noise_cov[2] / 2,
-                    0,
-                    0,
-                    dt2 * self.process_noise_cov[2],
-                ],
-            ]
-        ).astype(np.float32)
+
+        Q = np.array([[dt4*self.process_noise_cov[0]/4, 0, 0, dt3*self.process_noise_cov[0]/2, 0, 0],
+                      [0, dt4*self.process_noise_cov[1]/4, 0, 0, dt3*self.process_noise_cov[1]/2, 0],
+                      [0, 0, dt4*self.process_noise_cov[2]/4, 0, 0, dt3*self.process_noise_cov[2]/2],
+                      [dt3*self.process_noise_cov[0]/2, 0, 0, dt2*self.process_noise_cov[0], 0, 0],
+                      [0, dt3*self.process_noise_cov[1]/2, 0, 0, dt2*self.process_noise_cov[1], 0],
+                      [0, 0, dt3*self.process_noise_cov[2]/2, 0, 0, dt2*self.process_noise_cov[2]]]).astype(np.float32)
 
         self.kalman.transitionMatrix = F
         self.kalman.processNoiseCov = Q