38 lines
1.1 KiB
C++
38 lines
1.1 KiB
C++
#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
|