From 50732a2a85fd2c5934bc5aba1e4365a31a7d9372 Mon Sep 17 00:00:00 2001 From: Eric Huang Date: Wed, 10 Jun 2026 19:49:38 -0400 Subject: [PATCH 1/2] fix: correct FpgaInterface register access in sim mode Subtract FPGA_BASE_ADDR before indexing into the registers array. offset/4 was computing index ~285M into a 128-element array, causing an immediate segfault in drone_control_sim. Also correct for real hardware mmap path where registers_ points to the base of the mapping. --- arm/drone_main.cpp | 46 ++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 40 insertions(+), 6 deletions(-) diff --git a/arm/drone_main.cpp b/arm/drone_main.cpp index a4a8b6f..f8555de 100644 --- a/arm/drone_main.cpp +++ b/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 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) { From e52d8a3be9ceab7d8b1f285cdcbada85608c402a Mon Sep 17 00:00:00 2001 From: Eric Huang Date: Wed, 10 Jun 2026 19:49:51 -0400 Subject: [PATCH 2/2] feat(arm): integrate KalmanTracker into main control loop Wire kalman_tracker.h into the 100Hz control loop (Step 2b). Confirmed tracks (age >= 3 frames) replace raw CollisionPredictor output before EvasionController sees it, suppressing single-frame noise from triggering evasive maneuvers. - Add tracker.update() + get_confirmed_tracks() after predictor.assess() - Rebuild assessment.objects with KF-filtered positions - Add trk=/conf= columns to log_telemetry for flight debugging - Add make test target + test_arm_cp.cpp (6 unit tests, all passing) - Fix missing kalman_tracker.h dependency in Makefile %.o rule --- arm/Makefile | 18 ++- arm/test_arm_cp.cpp | 354 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 368 insertions(+), 4 deletions(-) create mode 100644 arm/test_arm_cp.cpp diff --git a/arm/Makefile b/arm/Makefile index 17ebe91..8630348 100644 --- a/arm/Makefile +++ b/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 "" diff --git a/arm/test_arm_cp.cpp b/arm/test_arm_cp.cpp new file mode 100644 index 0000000..30e0d54 --- /dev/null +++ b/arm/test_arm_cp.cpp @@ -0,0 +1,354 @@ +// arm/test_arm_cp.cpp — Kalman tracker integration tests +// +// Tests: +// Lifecycle : confirmation threshold, max_age pruning, empty-detections invariant +// Accuracy : two-object separation, Kalman position smoothing +// Integration: drone_main.cpp Step 2b threat gate (exact replication) +// +// Build + run: make test +// Binary: ./test_arm_cp (exit 0 = all pass, 1 = any failure) + +#include +#include +#include +#include + +// Include order matters: kalman_tracker.h uses ObjectCluster but does not +// include collision_predictor.h itself. Include predictor first. +#include "collision_predictor.h" +#include "kalman_tracker.h" + +using namespace drone; + +// --------------------------------------------------------------------------- +// Minimal test framework (no external dependencies) +// --------------------------------------------------------------------------- +static int g_total = 0; +static int g_passed = 0; +static bool g_test_failed = false; +static const char* g_test_name = ""; + +#define RUN_TEST(name) \ + do { g_test_name = #name; g_test_failed = false; g_total++; \ + printf(" [ RUN ] %s\n", g_test_name); } while(0) + +#define END_TEST \ + do { if (!g_test_failed) { \ + printf(" [ PASS ] %s\n\n", g_test_name); g_passed++; \ + } else { \ + printf(" [ FAIL ] %s\n\n", g_test_name); } } while(0) + +#define EXPECT_EQ(a, b) \ + do { if ((a) != (b)) { \ + printf(" EXPECT_EQ failed %s:%d\n left: %s = %d\n right: %s = %d\n", \ + __FILE__, __LINE__, #a, (int)(a), #b, (int)(b)); \ + g_test_failed = true; } } while(0) + +#define EXPECT_TRUE(cond) \ + do { if (!(cond)) { \ + printf(" EXPECT_TRUE failed %s:%d: %s\n", __FILE__, __LINE__, #cond); \ + g_test_failed = true; } } while(0) + +#define EXPECT_FALSE(cond) \ + do { if ((cond)) { \ + printf(" EXPECT_FALSE failed %s:%d: %s\n", __FILE__, __LINE__, #cond); \ + g_test_failed = true; } } while(0) + +#define EXPECT_NEAR(a, b, tol) \ + do { float _diff = std::abs((float)(a) - (float)(b)); \ + if (_diff > (float)(tol)) { \ + printf(" EXPECT_NEAR failed %s:%d\n |%s - %s| = %.5f > %.5f\n", \ + __FILE__, __LINE__, #a, #b, _diff, (float)(tol)); \ + g_test_failed = true; } } while(0) + +// --------------------------------------------------------------------------- +// Test helpers +// --------------------------------------------------------------------------- + +// Build a minimal ObjectCluster at (cx, cy) with realistic defaults +static ObjectCluster make_cluster(float cx, float cy, + float urgency = 0.8f, float ttc = 0.5f) { + ObjectCluster c{}; + c.id = 0; + c.center_x = cx; + c.center_y = cy; + c.spatial_extent = 0.05f; + c.mean_flow_mag = 1.0f; + c.mean_flow_dir = 0.0f; + c.ttc = ttc; + c.collision_urgency = urgency; + c.event_count = 50; + return c; +} + +// Replicate drone_main.cpp Step 2b exactly. +// Returns a ThreatAssessment post-tracker: objects replaced with KF-filtered +// confirmed tracks, threat_detected recomputed from confirmed track count. +static ThreatAssessment run_integration_step( + KalmanTracker& tracker, + std::vector& tracks, + std::vector raw_objects, + bool raw_threat = true) +{ + ThreatAssessment assessment; + assessment.objects = raw_objects; + assessment.threat_detected = raw_threat; + assessment.max_urgency = raw_threat ? 0.8f : 0.0f; + assessment.safe_bearing = 0.0f; + assessment.safe_bearing_confidence = 1.0f; + assessment.time_to_first_collision = + raw_threat ? 0.5f : std::numeric_limits::max(); + + // --- drone_main.cpp Step 2b (verbatim) --- + tracker.update(assessment.objects, tracks); + + auto confirmed = tracker.get_confirmed_tracks(tracks); + + assessment.objects.clear(); + for (const auto& t : confirmed) { + ObjectCluster oc = t.latest_cluster; + oc.center_x = t.kf_state.x; + oc.center_y = t.kf_state.y; + assessment.objects.push_back(oc); + } + assessment.threat_detected = !confirmed.empty(); + // ----------------------------------------- + + return assessment; +} + +// --------------------------------------------------------------------------- +// Test 1: Confirmation requires exactly min_hits_to_confirm (default 3) hits. +// +// Verifies that a track is NOT visible to the evasion controller on frames +// 1 and 2, but IS visible on frame 3. Prevents single-frame noise from +// triggering evasive manoeuvres. +// --------------------------------------------------------------------------- +void test_confirm_requires_3_hits() { + RUN_TEST(test_confirm_requires_3_hits); + + KalmanTracker tracker; + std::vector tracks; + std::vector det = { make_cluster(0.5f, 0.5f) }; + + // Frame 1 — age=1, below threshold (< 3) + tracker.update(det, tracks); + EXPECT_EQ(tracks.size(), 1u); + EXPECT_EQ(tracker.get_confirmed_tracks(tracks).size(), 0u); + + // Frame 2 — age=2, still below threshold + tracker.update(det, tracks); + EXPECT_EQ(tracks.size(), 1u); + EXPECT_EQ(tracker.get_confirmed_tracks(tracks).size(), 0u); + + // Frame 3 — age=3, now confirmed (age >= min_hits_to_confirm) + tracker.update(det, tracks); + EXPECT_EQ(tracks.size(), 1u); + EXPECT_EQ(tracker.get_confirmed_tracks(tracks).size(), 1u); + + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 2: Track is pruned after max_age (default 30) consecutive empty frames. +// +// Verifies the exact boundary: +// - 30 empty frames: frames_since_update = 30, condition (> 30) is false → alive +// - 31 empty frames: frames_since_update = 31, condition (> 30) is true → pruned +// +// This ensures stale tracks from occluded objects don't persist indefinitely +// and never cause phantom evasion responses. +// --------------------------------------------------------------------------- +void test_track_pruned_after_max_age() { + RUN_TEST(test_track_pruned_after_max_age); + + KalmanTracker tracker; + std::vector tracks; + std::vector det = { make_cluster(0.5f, 0.5f) }; + std::vector empty = {}; + + // Confirm the track (3 hits, frames_since_update reset to 0 on last match) + for (int i = 0; i < 3; i++) tracker.update(det, tracks); + EXPECT_EQ(tracks.size(), 1u); + + // 30 empty frames: track should still be alive at exactly the boundary + for (int i = 0; i < 30; i++) tracker.update(empty, tracks); + EXPECT_EQ(tracks.size(), 1u); + + // 31st empty frame: frames_since_update = 31 > 30 → pruned + tracker.update(empty, tracks); + EXPECT_EQ(tracks.size(), 0u); + + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 3: Calling update() with empty detections never creates tracks. +// +// Verifies the always-call invariant: the main loop calls tracker.update() +// every tick (even when n_events == 0) so stale tracks age correctly. +// A side effect must never be track creation. +// --------------------------------------------------------------------------- +void test_no_detections_no_tracks() { + RUN_TEST(test_no_detections_no_tracks); + + KalmanTracker tracker; + std::vector tracks; + std::vector empty = {}; + + for (int i = 0; i < 20; i++) tracker.update(empty, tracks); + + EXPECT_EQ(tracks.size(), 0u); + EXPECT_EQ(tracker.get_confirmed_tracks(tracks).size(), 0u); + + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 4: Two spatially separated detections produce two independent tracks. +// +// Verifies that Mahalanobis gating prevents cross-association when objects +// are 0.8 apart. Positions are checked after confirmation so we also verify +// that each track converged toward its own detection, not a blend. +// --------------------------------------------------------------------------- +void test_two_objects_two_tracks() { + RUN_TEST(test_two_objects_two_tracks); + + KalmanTracker tracker; + std::vector tracks; + std::vector dets = { + make_cluster(0.1f, 0.1f, 0.9f), + make_cluster(0.9f, 0.9f, 0.7f), + }; + + // 3 frames to confirm both tracks + for (int i = 0; i < 3; i++) tracker.update(dets, tracks); + + EXPECT_EQ(tracks.size(), 2u); + EXPECT_EQ(tracker.get_confirmed_tracks(tracks).size(), 2u); + + // Each confirmed track must be near its original detection, not the other one + auto conf = tracker.get_confirmed_tracks(tracks); + bool found_near_01 = false, found_near_09 = false; + for (const auto& t : conf) { + if (std::abs(t.kf_state.x - 0.1f) < 0.05f && + std::abs(t.kf_state.y - 0.1f) < 0.05f) + found_near_01 = true; + if (std::abs(t.kf_state.x - 0.9f) < 0.05f && + std::abs(t.kf_state.y - 0.9f) < 0.05f) + found_near_09 = true; + } + EXPECT_TRUE(found_near_01); + EXPECT_TRUE(found_near_09); + + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 5: Kalman filter reduces position noise below raw measurement noise. +// +// Feeds alternating +/- 0.04 noise around a true position of (0.5, 0.5). +// After 30 frames the KF estimate should have smaller error than the raw +// noise amplitude — confirming the filter is actively smoothing and not +// just passing measurements through. +// --------------------------------------------------------------------------- +void test_kalman_smoothing() { + RUN_TEST(test_kalman_smoothing); + + KalmanTracker tracker; + std::vector tracks; + const float true_pos = 0.5f; + const float noise = 0.04f; + + for (int i = 0; i < 30; i++) { + float sign = (i % 2 == 0) ? 1.0f : -1.0f; + std::vector det = { + make_cluster(true_pos + sign * noise, true_pos + sign * noise) + }; + tracker.update(det, tracks); + } + + auto conf = tracker.get_confirmed_tracks(tracks); + EXPECT_EQ(conf.size(), 1u); + + if (!conf.empty()) { + // Filtered error should be strictly less than the raw noise amplitude + float err_x = std::abs(conf[0].kf_state.x - true_pos); + float err_y = std::abs(conf[0].kf_state.y - true_pos); + EXPECT_TRUE(err_x < noise); + EXPECT_TRUE(err_y < noise); + } + + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test 6: Integration — threat gate replicates drone_main.cpp Step 2b. +// +// This test runs the exact integration logic we added to drone_main.cpp: +// - Frames 1-2: raw threat present, but no confirmed tracks yet +// → assessment.threat_detected must be false +// → assessment.objects must be empty (evader is not triggered) +// - Frame 3: track confirmed +// → assessment.threat_detected must be true +// → assessment.objects has 1 entry with KF-filtered position +// - After max_age+1 empty frames: track pruned +// → assessment.threat_detected must be false again +// --------------------------------------------------------------------------- +void test_integration_threat_gate() { + RUN_TEST(test_integration_threat_gate); + + KalmanTracker tracker; + std::vector tracks; + std::vector det = { make_cluster(0.5f, 0.5f, 0.8f) }; + std::vector empty = {}; + + // Frames 1 and 2: raw threat, but evader must not see it yet + for (int frame = 1; frame <= 2; frame++) { + auto result = run_integration_step(tracker, tracks, det, true); + EXPECT_FALSE(result.threat_detected); + EXPECT_EQ(result.objects.size(), 0u); + } + + // Frame 3: confirmed — evader now sees the threat + { + auto result = run_integration_step(tracker, tracks, det, true); + EXPECT_TRUE(result.threat_detected); + EXPECT_EQ(result.objects.size(), 1u); + if (!result.objects.empty()) { + // Reported position should be close to the detection (KF hasn't drifted far) + EXPECT_NEAR(result.objects[0].center_x, 0.5f, 0.02f); + EXPECT_NEAR(result.objects[0].center_y, 0.5f, 0.02f); + } + } + + // max_age+1 empty frames: track must be pruned, threat cleared + ThreatAssessment last; + for (int i = 0; i <= 31; i++) { + last = run_integration_step(tracker, tracks, empty, false); + } + EXPECT_FALSE(last.threat_detected); + EXPECT_EQ(last.objects.size(), 0u); + + END_TEST; +} + +// --------------------------------------------------------------------------- +// Test runner +// --------------------------------------------------------------------------- +int main() { + printf("ARM Kalman Tracker — Integration Tests\n"); + printf("=======================================\n\n"); + + test_confirm_requires_3_hits(); + test_track_pruned_after_max_age(); + test_no_detections_no_tracks(); + test_two_objects_two_tracks(); + test_kalman_smoothing(); + test_integration_threat_gate(); + + printf("=======================================\n"); + printf("Results: %d/%d passed\n\n", g_passed, g_total); + + return (g_passed == g_total) ? 0 : 1; +}