fix headless sim bugs

filter performance is still abysmal

.gitignore

@@ -5,5 +5,5 @@ __pycache__
 *.log
 runs
 runs_archive
-.vscode
+.vscode/*
 !.vscode/launch.json

.vscode/launch.json

@@ -25,6 +25,14 @@
             "module": "src.component_algos.test_rocket_filter",
             "justMyCode": true
         },
+        {
+            "name": "Python: debug module src.headless_sim",
+            "type": "python",
+            "request": "launch",
+            "console": "integratedTerminal",
+            "module": "src.headless_sim",
+            "justMyCode": true
+        },
         {
             "name": "Python Debugger: Python File",
             "type": "debugpy",

simulation/rocket.py

@@ -2,6 +2,7 @@
 from src.utils import TelemetryData
 import numpy as np
 import pymap3d as pm
+from scipy.spatial.transform import Rotation as R
 
 class Rocket:
     def __init__(self, pad_geodetic_pos: np.ndarray, launch_time: float = 0):
@@ -60,7 +61,7 @@ def get_telemetry(self, time):
             time = time
         )
 
-    def get_quarternion(self, time):
-        return np.array([
-            test_flight.e0(time), test_flight.e1(time), test_flight.e2(time), test_flight.e3(time), 
-        ])
+    def get_orientation(self, time) -> R:
+        return R.from_quat(np.array([
+            test_flight.e0(time), test_flight.e1(time), test_flight.e2(time), test_flight.e3(time)
+        ]), scalar_first=True)

src/component_algos/rocket_filter.py

@@ -3,6 +3,7 @@
 from torch.utils.tensorboard import SummaryWriter
 from filterpy.kalman import UnscentedKalmanFilter, MerweScaledSigmaPoints
 from itertools import product
+from src.component_algos.depth_of_field import MM_PER_PIXEL
 
 #https://kodlab.seas.upenn.edu/uploads/Arun/UKFpaper.pdf
 
@@ -19,12 +20,12 @@ def _copy_helper(obj, new_obj): # assign all float and ndarray attributes from o
 
 class RocketFilter:
     STATE_DIM = 8
-    ROCKET_HEIGHT = 8
-    FOCAL_LEN = 177.87 # TODO: un-hardcode this
+    ROCKET_HEIGHT = 6.5
     def __init__(self, 
                 pad_geodetic_location: tuple[float,float,float], 
                 cam_geodetic_location: tuple[float,float,float],
                 initial_cam_orientation: tuple[float,float],
+                focal_len_px: float,
                 drag_coefficient: float = 5e-4,
                 launch_time = None,
                 writer: SummaryWriter = None):
@@ -48,6 +49,7 @@ def __init__(self,
         self.drag_coefficient = 0#drag_coefficient
         self.initial_cam_orientation = initial_cam_orientation
         self.writer = writer
+        self.focal_len_px = focal_len_px
 
         self._launch_time = launch_time
         self._last_update_time = launch_time
@@ -79,7 +81,7 @@ def __init__(self,
         telem_measurement_std = np.array([10,10,10,1])
         self.R_telem = np.diag(np.square(telem_measurement_std)) # measurement noise covariance matrix
 
-        bearing_measurement_std = np.array([1e-5, 1e-5, 10])
+        bearing_measurement_std = np.array([1e-5, 1e-5, 1])
         self.R_bearing = np.diag(np.square(bearing_measurement_std)) # measurement noise covariance matrix
 
 
@@ -134,9 +136,8 @@ def hx_bearing(self, x: np.ndarray):
         elevation_bearing = np.rad2deg(np.arctan2(rocket_pos_enu[2], np.linalg.norm(rocket_pos_enu[:2])))
 
         # TODO: rewrite this so it uses better math than similar triangles which assumes the rocket is in the center of the frame
-        focal_len_px = RocketFilter.FOCAL_LEN
         dist_to_cam = np.linalg.norm(rocket_pos_enu)
-        apparent_size = focal_len_px * RocketFilter.ROCKET_HEIGHT / dist_to_cam
+        apparent_size = self.focal_len_px * RocketFilter.ROCKET_HEIGHT / dist_to_cam
 
         return np.array([
             azimuth_bearing,
@@ -195,6 +196,8 @@ def _log_state(self, time: float):
             self.writer.add_scalar(f'ukf/x_{i}', x, time*100)
         for i,j in product(range(self.P.shape[0]), range(self.P.shape[1])):
             self.writer.add_scalar(f'ukf/P_{i}_{j}', self.P[i,j], time*100)
+        self.writer.add_scalar('ukf/bearing_log_likelihood', self.bearing_ukf.log_likelihood, time*100)
+        self.writer.add_scalar('ukf/telem_log_likelihood', self.telem_ukf.log_likelihood, time*100)
 
     def predict_update_bearing(self, time: float, z: np.ndarray):
         '''

src/headless_sim.py

@@ -16,7 +16,7 @@
 
 class HeadlessSim:
     def __init__(self):
-        self.camera_res = (1920, 1080)
+        self.camera_res = (1920//2, 1080)
         self.camera_fov = 0.9
         focal_len_pixels = self.camera_res[0]/(2*np.tan(np.deg2rad(self.camera_fov/2)))
         # fov should be around 0.9 degrees IRL
@@ -76,15 +76,14 @@ def estimate_pos(tracker_self, img: np.ndarray) -> tuple[int, int, int, int]:
                 '''
 
                 rocket_ecef_pos = self.rocket.get_position_ecef(self.time - self.launch_time)
-                rocket_orientation = self.rocket.get_quarternion(self.time - self.launch_time)
                 rocket_enu_camera_frame_pos = pm.ecef2enu(*rocket_ecef_pos, *self.environment.get_cam_pos_gps())                
 
                 # reference points are the top and bottom of the rocket plus or minus the rocket radius
                 # this will assume that the rocket is pointed in the direction of its velocity vector
                 rocket_radius = 0.9
                 rocket_height = 6.5
-                transform = R.from_quat(rocket_orientation)
-                vel_direction = transform.apply([0,0,1])
+                rocket_orientation = self.rocket.get_orientation(self.time - self.launch_time)
+                vel_direction = rocket_orientation.apply([0,0,1])
 
                 top_point = rocket_enu_camera_frame_pos + rocket_height/2 * vel_direction
                 bottom_point = rocket_enu_camera_frame_pos - rocket_height/2 * vel_direction
@@ -119,7 +118,9 @@ def estimate_pos(tracker_self, img: np.ndarray) -> tuple[int, int, int, int]:
 
                 min_x, min_y = np.min(pixel_coords, axis=0)
                 max_x, max_y = np.max(pixel_coords, axis=0)
-                return min_x, min_y, max_x, max_y
+                center_x = int((min_x+max_x)/2)
+                center_y = int((min_y+max_y)/2)
+                return center_x, center_y, int(max_x-min_x), int(max_y-min_y)
 
         self.img_tracker = MockImageTracker()
 
@@ -170,6 +171,9 @@ def step(self, step_time: float):
         self.logger.add_scalar("enu position/y", y, step_time*100)
         self.logger.add_scalar("enu position/z", z, step_time*100)
 
+        dist_to_camera = np.linalg.norm(rocket_pos_ecef - np.array(pm.geodetic2ecef(*self.cam_geodetic_location)))
+        self.logger.add_scalar('mount/distance', dist_to_camera, step_time*100)
+
         self.logger.add_scalar("enu velocity/x", rocket_vel[0], step_time*100)
         self.logger.add_scalar("enu velocity/y", rocket_vel[1], step_time*100)
         self.logger.add_scalar("enu velocity/z", rocket_vel[2], step_time*100)
@@ -181,6 +185,9 @@ def step(self, step_time: float):
         pixel_x, pixel_y = bbox[:2]
         self.logger.add_scalar("pixel position/x", pixel_x, step_time*100)
         self.logger.add_scalar("pixel position/y", pixel_y, step_time*100)
+        self.logger.add_scalar("pixel position/bbox_w", bbox[2], step_time*100)
+        self.logger.add_scalar("pixel position/bbox_h", bbox[3], step_time*100)
+        self.logger.add_scalar("pixel position/bbox_diag", np.linalg.norm(bbox[2:]), step_time*100)
 
         az_new, alt_new = self._pixel_pos_to_az_alt(bbox[:2])
         self.logger.add_scalar("bearing/azimuth", az_new, step_time*100)

src/tracker.py

@@ -62,7 +62,7 @@ def _ecef_to_az_alt(self, ecef_pos: np.ndarray) -> tuple[float,float]:
 
     def start_tracking(self, initial_cam_orientation: tuple[float,float]):
         self.initial_cam_orientation = initial_cam_orientation
-        self.filter = RocketFilter(self.environment.get_pad_pos_gps(), self.gps_pos, self.initial_cam_orientation, writer=self.logger)
+        self.filter = RocketFilter(self.environment.get_pad_pos_gps(), self.gps_pos, self.initial_cam_orientation, self.focal_len_pixels, writer=self.logger)
         self.launch_detector = None
         self.active_tracking = True
         self.img_tracker.start_new_tracking()
@@ -116,10 +116,12 @@ def update_tracking(self, img: np.ndarray, telem_measurements: TelemetryData, ti
             self.filter.predict_update_bearing(time, np.array([az, alt, bbox_diagonal_len]))
 
 
-        predicted_pixel_pos = self._az_alt_to_pixel_pos(self.filter.hx_bearing(self.filter.x)[:2])
+        pred_measurement = self.filter.hx_bearing(self.filter.x)
+        predicted_pixel_pos = self._az_alt_to_pixel_pos(pred_measurement[:2])
         cv.circle(img, predicted_pixel_pos, 10, (255,0,0), 2)
         self.logger.add_scalar("pixel position/x_filter", predicted_pixel_pos[0], time*100)
         self.logger.add_scalar("pixel position/y_filter", predicted_pixel_pos[1], time*100)
+        self.logger.add_scalar("pixel position/bbox_diag", pred_measurement[2], time*100)
 
         if telem_measurements is not None:
             ecef_pos = pm.geodetic2ecef(telem_measurements.gps_lat, telem_measurements.gps_lng, telem_measurements.altimeter_reading)