feat(arm): integrate KalmanTracker into main control loop

arm/Makefile

@@ -45,6 +45,7 @@ INCLUDES = -I. -I$(FREERTOS_PATH)/include -I$(FREERTOS_PATH)/portable/GCC/ARM_CR
 # ---------------------------------------------------------------------------
 TARGET = drone_control
 TARGET_SIM = drone_control_sim
+TEST_TARGET = test_arm_cp
 
 # Source files (header-only for collision_predictor.h, evasion_controller.h)
 SOURCES = drone_main.cpp
@@ -55,7 +56,7 @@ OBJECTS = $(SOURCES:.cpp=.o)
 # ---------------------------------------------------------------------------
 # Build rules
 # ---------------------------------------------------------------------------
-.PHONY: all sim freertos clean
+.PHONY: all sim freertos test clean
 
 # Default: hardware target
 all: $(TARGET)
@@ -85,12 +86,21 @@ freertos: $(OBJECTS)
 	@echo "Built $(TARGET)_rtos.elf for FreeRTOS"
 
 # Compile source files
-%.o: %.cpp collision_predictor.h evasion_controller.h
+%.o: %.cpp collision_predictor.h evasion_controller.h kalman_tracker.h
 	$(CXX) $(CXXFLAGS) $(INCLUDES) -c $< -o $@
 
+# Unit tests (native g++ — runs on dev machine, no ARM cross-compiler needed)
+# Output: ./test_arm_cp  exit 0 = all pass, 1 = any failure
+test: $(TEST_TARGET)
+	./$(TEST_TARGET)
+
+$(TEST_TARGET): test_arm_cp.cpp collision_predictor.h evasion_controller.h kalman_tracker.h
+	g++ -std=c++17 -O0 -g -Wall -Wextra -I. -o $(TEST_TARGET) test_arm_cp.cpp
+	@echo "Built $(TEST_TARGET)"
+
 # Clean
 clean:
-	rm -f $(TARGET) $(TARGET_SIM) $(TARGET)_rtos.elf *.o
+	rm -f $(TARGET) $(TARGET_SIM) $(TARGET)_rtos.elf $(TEST_TARGET) *.o
 	@echo "Clean complete"
 
 # ---------------------------------------------------------------------------
@@ -115,7 +125,7 @@ help:
 	@echo "  make          Build binary for Zynq ARM Linux"
 	@echo "  make sim      Build simulation binary (run on desktop, no FPGA needed)"
 	@echo "  make freertos Build bare-metal binary for Zynq R5 with FreeRTOS"
-	@echo "  make clean    Remove build artifacts"
+	@echo "  make test     Build and run unit tests (native g++, no FPGA needed)"
 	@echo "  make deploy   SCP binary to Zynq board"
 	@echo "  make run      SSH into Zynq and run binary"
 	@echo ""

arm/drone_main.cpp

@@ -25,6 +25,7 @@
 
 #include "collision_predictor.h"
 #include "evasion_controller.h"
+#include "kalman_tracker.h"
 
 // FreeRTOS or bare-metal alternatives
 #ifdef FREERTOS
@@ -86,11 +87,11 @@ class FpgaInterface {
     }
 
     void write_register(uint32_t offset, uint32_t value) {
-        registers_[offset / 4] = value;
+        registers_[(offset - FPGA_BASE_ADDR) / 4] = value;
     }
 
     uint32_t read_register(uint32_t offset) {
-        return registers_[offset / 4];
+        return registers_[(offset - FPGA_BASE_ADDR) / 4];
     }
 
     // -----------------------------------------------------------------
@@ -182,7 +183,8 @@ class FpgaInterface {
 // ---------------------------------------------------------------------------
 // Telemetry / logging
 // ---------------------------------------------------------------------------
-void log_telemetry(const ThreatAssessment& assess, const EvasionCommand& cmd) {
+void log_telemetry(const ThreatAssessment& assess, const EvasionCommand& cmd,
+                   size_t n_tracks, size_t n_confirmed) {
     static uint32_t frame = 0;
     frame++;
 
@@ -198,9 +200,10 @@ void log_telemetry(const ThreatAssessment& assess, const EvasionCommand& cmd) {
         printf("CLEAR                               | ");
     }
 
-    printf("%-9s vx=%+5.2f vy=%+5.2f vz=%+5.2f yaw=%+5.2f\n",
+    printf("%-9s vx=%+5.2f vy=%+5.2f vz=%+5.2f yaw=%+5.2f | trk=%zu conf=%zu\n",
            evasion_level_name(cmd.level),
-           cmd.velocity_x, cmd.velocity_y, cmd.velocity_z, cmd.yaw_rate);
+           cmd.velocity_x, cmd.velocity_y, cmd.velocity_z, cmd.yaw_rate,
+           n_tracks, n_confirmed);
 }
 
 // ---------------------------------------------------------------------------
@@ -241,6 +244,15 @@ int main(int /*argc*/, char** /*argv*/) {
     evade_cfg.repulsion_strength = 1.0f;
     EvasionController evader(evade_cfg);
 
+    // Initialize Kalman tracker
+    // Config defaults are tuned for 100Hz — no overrides needed:
+    //   dt=0.01s, process_noise=0.005, meas_noise=0.05
+    //   max_age=30 frames (0.3s), min_hits_to_confirm=3 frames (30ms)
+    KalmanTracker tracker;
+
+    // Persistent track list — lives across loop iterations, passed by ref into tracker.update()
+    std::vector<TrackedObject> tracks;
+
     // Control loop timing
     const int CONTROL_FREQ_HZ = 100;  // 100Hz control loop
     const int CONTROL_PERIOD_MS = 1000 / CONTROL_FREQ_HZ;
@@ -269,6 +281,28 @@ int main(int /*argc*/, char** /*argv*/) {
             assessment = predictor.assess(events);
         }
 
+        // Step 2b: Kalman tracker — always called, even with zero detections.
+        // Empty detections = no new tracks created, but existing tracks age toward pruning.
+        // Skipping this when n_events==0 would make tracks immortal during quiet frames.
+        tracker.update(assessment.objects, tracks);
+
+        // Confirmed tracks have seen >= 3 consecutive hits (30ms at 100Hz).
+        // Single-frame blips are suppressed — the evasion controller never sees them.
+        auto confirmed = tracker.get_confirmed_tracks(tracks);
+
+        // Replace raw detections with Kalman-filtered confirmed-track positions.
+        // Each ObjectCluster retains its original TTC/urgency metadata (latest_cluster)
+        // but center_x/y are overridden with the smoothed KF estimate.
+        assessment.objects.clear();
+        for (const auto& t : confirmed) {
+            ObjectCluster oc = t.latest_cluster;  // carries TTC, flow, size metadata
+            oc.center_x = t.kf_state.x;           // KF-filtered position
+            oc.center_y = t.kf_state.y;
+            assessment.objects.push_back(oc);
+        }
+        // threat_detected now reflects confirmed track presence, not raw noisy detections
+        assessment.threat_detected = !confirmed.empty();
+
         // Step 3: Compute evasion command
         EvasionCommand cmd = evader.compute_command(assessment);
 
@@ -278,7 +312,7 @@ int main(int /*argc*/, char** /*argv*/) {
         }
 
         // Step 5: Telemetry
-        log_telemetry(assessment, cmd);
+        log_telemetry(assessment, cmd, tracks.size(), confirmed.size());
 
         // Auto-arm after startup countdown
         if (!g_motors_armed && arm_countdown > 0) {