参考上海交通大学代码进行修改

Catalyst-MDVS2/inc/KalmanFilter.h

@@ -0,0 +1,38 @@
+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include <opencv2/opencv.hpp>
+
+class KalmanFilter {
+public:
+    KalmanFilter();
+
+    void update(const cv::Point2f& measurement);
+    cv::Point2f predict();
+
+    // Method to update the process noise based on target movement characteristics
+    void updateProcessNoise(const cv::Point2f& current_measurement);
+
+    cv::Point2f get_last_measurement() const { return last_measurement; }
+    cv::Point2f get_last_prediction() const { return last_prediction; }
+    cv::Point2f get_current_velocity() const;  // New method to get current velocity
+    bool is_initialized() const { return initialized; }
+
+private:
+    cv::KalmanFilter kf;
+    bool initialized;
+    cv::Point2f last_measurement;
+    cv::Point2f last_prediction;
+    cv::Point2f prev_measurement;
+    bool has_prev_measurement;
+
+    // Store previous states for velocity and acceleration estimation
+    cv::Point2f prev_velocity;
+    double time_elapsed;  // Time in seconds between measurements
+    bool has_prev_velocity;
+
+    void init_params();
+    cv::Point2f estimate_velocity(const cv::Point2f& current, const cv::Point2f& previous);
+};
+
+#endif // KALMANFILTER_H