Added Covariance ellipse as well as Predicted and corrected position.

steps/index.html

@@ -154,6 +154,14 @@
           <label class="flex items-center gap-2 text-sm cursor-pointer"><input type="checkbox" id="toggle-close-up"
               class="form-checkbox h-4 w-4 text-blue-600 rounded focus:ring-blue-500" />
             CLOSE-UP</label>
+          <label class="flex items-center gap-2 text-sm cursor-pointer">
+            <input type="checkbox" id="toggle-predicted-pos"
+              class="form-checkbox h-4 w-4 text-blue-600 rounded focus:ring-blue-500" />
+            Show Predicted Position</label>
+          <label class="flex items-center gap-2 text-sm cursor-pointer">
+            <input type="checkbox" id="toggle-covariance"
+              class="form-checkbox h-4 w-4 text-blue-600 rounded focus:ring-blue-500" />
+            Show Covariance</label>
         </div>
         <div class="flex items-center gap-4">
           <div class="flex items-center gap-2">

steps/src/dom.js

@@ -23,39 +23,25 @@ export const speedSlider = document.getElementById("speed-slider");
 export const speedDisplay = document.getElementById("speed-display");
 export const featureToggles = document.getElementById("feature-toggles");
 export const toggleSnrColor = document.getElementById("toggle-snr-color");
-export const toggleClusterColor = document.getElementById(
-  "toggle-cluster-color"
-);
+export const toggleClusterColor = document.getElementById("toggle-cluster-color");
 export const toggleInlierColor = document.getElementById("toggle-inlier-color");
-export const toggleStationaryColor = document.getElementById(
-  "toggle-stationary-color"
-);
+export const toggleStationaryColor = document.getElementById("toggle-stationary-color");
 export const toggleVelocity = document.getElementById("toggle-velocity");
 export const toggleTracks = document.getElementById("toggle-tracks");
 export const toggleEgoSpeed = document.getElementById("toggle-ego-speed");
 export const toggleFrameNorm = document.getElementById("toggle-frame-norm");
-export const toggleDebugOverlay = document.getElementById(
-  "toggle-debug-overlay"
-);
+export const toggleDebugOverlay = document.getElementById("toggle-debug-overlay");
 export const egoSpeedDisplay = document.getElementById("ego-speed-display");
 export const canSpeedDisplay = document.getElementById("can-speed-display");
 export const debugOverlay = document.getElementById("debug-overlay");
-export const toggleDebug2Overlay = document.getElementById(
-  "toggle-debug2-overlay"
-);
+export const toggleDebug2Overlay = document.getElementById("toggle-debug2-overlay");
 export const snrMinInput = document.getElementById("snr-min-input");
 export const snrMaxInput = document.getElementById("snr-max-input");
 export const applySnrBtn = document.getElementById("apply-snr-btn");
-export const autoOffsetIndicator = document.getElementById(
-  "auto-offset-indicator"
-);
+export const autoOffsetIndicator = document.getElementById("auto-offset-indicator");
 export const clearCacheBtn = document.getElementById("clear-cache-btn");
-export const speedGraphContainer = document.getElementById(
-  "speed-graph-container"
-);
-export const speedGraphPlaceholder = document.getElementById(
-  "speed-graph-placeholder"
-);
+export const speedGraphContainer = document.getElementById("speed-graph-container");
+export const speedGraphPlaceholder = document.getElementById("speed-graph-placeholder");
 export const modalContainer = document.getElementById("modal-container");
 export const modalOverlay = document.getElementById("modal-overlay");
 export const modalContent = document.getElementById("modal-content");
@@ -63,6 +49,8 @@ export const modalText = document.getElementById("modal-text");
 export const modalOkBtn = document.getElementById("modal-ok-btn");
 export const modalCancelBtn = document.getElementById("modal-cancel-btn");
 export const toggleCloseUp = document.getElementById("toggle-close-up");
+export const togglePredictedPos = document.getElementById("toggle-predicted-pos");
+export const toggleCovariance = document.getElementById("toggle-covariance");
 
 //----------------------UPDATE FRAME Function----------------------//
 // Updates the UI to reflect the current radar frame and synchronizes video playback.
@@ -71,7 +59,8 @@ export function updateFrame(frame, forceVideoSeek) {
     !appState.vizData ||
     frame < 0 ||
     frame >= appState.vizData.radarFrames.length
-  ) // Exit if no visualization data or invalid frame.
+  )
+    // Exit if no visualization data or invalid frame.
     return; // Exit if no visualization data or invalid frame
   appState.currentFrame = frame;
   timelineSlider.value = appState.currentFrame;
@@ -79,7 +68,8 @@ export function updateFrame(frame, forceVideoSeek) {
     appState.vizData.radarFrames.length
   }`;
   const frameData = appState.vizData.radarFrames[appState.currentFrame];
-  if (toggleEgoSpeed.checked && frameData) { // Update ego speed display if enabled.
+  if (toggleEgoSpeed.checked && frameData) {
+    // Update ego speed display if enabled.
     const egoVy_kmh = (frameData.egoVelocity[1] * 3.6).toFixed(1); // Convert m/s to km/h and format
     egoSpeedDisplay.textContent = `Ego: ${egoVy_kmh} km/h`;
     egoSpeedDisplay.classList.remove("hidden");
@@ -100,8 +90,10 @@ export function updateFrame(frame, forceVideoSeek) {
     const offsetMs = parseFloat(offsetInput.value) || 0;
     const targetRadarTimeMs = frameData.timestampMs;
     const targetVideoTimeSec = (targetRadarTimeMs - offsetMs) / 1000;
-    if (targetVideoTimeSec >= 0 && targetVideoTimeSec <= videoPlayer.duration) { // Ensure target time is within video duration
-      if (Math.abs(videoPlayer.currentTime - targetVideoTimeSec) > 0.05) { // Check for significant drift
+    if (targetVideoTimeSec >= 0 && targetVideoTimeSec <= videoPlayer.duration) {
+      // Ensure target time is within video duration
+      if (Math.abs(videoPlayer.currentTime - targetVideoTimeSec) > 0.05) {
+        // Check for significant drift
         videoPlayer.currentTime = targetVideoTimeSec; // Seek video if drift is significant
       }
       // MODIFIED: Use the calculated target time for our updates, not the stale videoPlayer.currentTime
@@ -117,7 +109,8 @@ export function updateFrame(frame, forceVideoSeek) {
   // --- End of fix ---
 
   if (appState.p5_instance) appState.p5_instance.redraw(); // Redraw radar sketch
-  if (appState.speedGraphInstance && !appState.isPlaying) // Redraw speed graph if not playing.
+  if (appState.speedGraphInstance && !appState.isPlaying)
+    // Redraw speed graph if not playing.
     appState.speedGraphInstance.redraw();
 }
 

steps/src/drawUtils.js

@@ -516,3 +516,38 @@ export function handleCloseUpDisplay(p, plotScales) {
     p.pop();
   }
 }
+
+export function drawCovarianceEllipse(p, position, covarianceP, plotScales) {
+  const pPos = [
+    [covarianceP[0][0], covarianceP[0][1]],
+    [covarianceP[1][0], covarianceP[1][1]],
+  ];
+
+  const a = pPos[0][0];
+  const b = pPos[0][1];
+  const d = pPos[1][1];
+  const trace = a + d;
+  const determinant = a * d - b * b;
+
+  const lambda1 = trace / 2 + Math.sqrt(Math.pow(trace, 2) / 4 - determinant);
+  const lambda2 = trace / 2 - Math.sqrt(Math.pow(trace, 2) / 4 - determinant);
+
+  const chi2 = 5.991;
+  const majorAxis = Math.sqrt(chi2 * lambda1);
+  const minorAxis = Math.sqrt(chi2 * lambda2);
+
+  let eigenvector = [1, 0];
+  if (b !== 0) {
+    eigenvector = [lambda1 - d, b];
+  }
+  const angle = Math.atan2(eigenvector[1], eigenvector[0]);
+
+  p.push();
+  p.noFill();
+  p.stroke(255, 0, 0, 150);
+  p.strokeWeight(1);
+  p.translate(position[0] * plotScales.plotScaleX, position[1] * plotScales.plotScaleY);
+  p.rotate(angle);
+  p.ellipse(0, 0, majorAxis * 2 * plotScales.plotScaleX, minorAxis * 2 * plotScales.plotScaleY);
+  p.pop();
+}

steps/src/p5/radarSketch.js

@@ -6,7 +6,13 @@ import {
   RADAR_Y_MAX,
   RADAR_Y_MIN,
 } from "../constants.js";
-import { canvasContainer, toggleSnrColor, toggleTracks } from "../dom.js";
+import {
+  canvasContainer,
+  toggleSnrColor,
+  toggleTracks,
+  togglePredictedPos,
+  toggleCovariance,
+} from "../dom.js";
 import {
   drawStaticRegionsToBuffer,
   drawAxes,
@@ -15,7 +21,8 @@ import {
   drawTrajectories,
   drawTrackMarkers,
   snrColors,
-  handleCloseUpDisplay, // BUG FIX 1: Import the close-up handler
+  handleCloseUpDisplay,
+  drawCovarianceEllipse, // BUG FIX 1: Import the close-up handler
 } from "../drawUtils.js";
 
 export const radarSketch = function (p) {
@@ -89,6 +96,44 @@ export const radarSketch = function (p) {
       if (toggleTracks.checked) {
         drawTrajectories(p, plotScales);
         drawTrackMarkers(p, plotScales);
+
+        if (toggleCovariance.checked) {
+          for (const track of appState.vizData.tracks) {
+            const log = track.historyLog.find(
+              (log) => log.frameIdx === appState.currentFrame + 1
+            );
+            if (log && log.covarianceP) {
+              const pos = log.predictedPosition;
+              if (pos && pos[0] !== null) {
+                drawCovarianceEllipse(p, pos, log.covarianceP, plotScales);
+              }
+            }
+          }
+        }
+
+        if (togglePredictedPos.checked) {
+          for (const track of appState.vizData.tracks) {
+            const log = track.historyLog.find(
+              (log) => log.frameIdx === appState.currentFrame + 1
+            );
+            if (
+              log &&
+              log.predictedPosition &&
+              log.predictedPosition[0] !== null
+            ) {
+              const pos = log.predictedPosition;
+              const x = pos[0] * plotScales.plotScaleX;
+              const y = pos[1] * plotScales.plotScaleY;
+
+              p.push();
+              p.stroke(255, 0, 0); // Red for predicted
+              p.strokeWeight(2);
+              p.line(x - 4, y - 4, x + 4, y + 4);
+              p.line(x + 4, y - 4, x - 4, y + 4);
+              p.pop();
+            }
+          }
+        }
       }
       // Draw the point cloud for the current frame
       drawPointCloud(p, frameData.pointCloud, plotScales);