Subversion Repositories Code-Repo

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter(float x, float y) {
        filter = cv::KalmanFilter(4, 2, 0);

        // Intialization of KF... what do these values represent??
        filter.transitionMatrix = *(cv::Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
        
        measurement = cv::Mat_<float>::zeros(2, 1);
        measurement(0) = x;
        measurement(1) = y;

        filter.statePre.at<float>(0) = x;
        filter.statePre.at<float>(1) = y;
        filter.statePre.at<float>(2) = 0;
        filter.statePre.at<float>(3) = 0;

        filter.statePost.at<float>(0) = x;
        filter.statePost.at<float>(1) = y;
        filter.statePost.at<float>(2) = 0;
        filter.statePost.at<float>(3) = 0;

        cv::setIdentity(filter.measurementMatrix);
        // Process noise covariance matrix, adjust for faster convergence but more noise
        cv::setIdentity(filter.processNoiseCov, cv::Scalar::all(PROCESS_NOISE_VARIANCE));
        // Measurement noise covariance matrix
        cv::setIdentity(filter.measurementNoiseCov, cv::Scalar::all(MEASUREMENT_NOISE_VARIANCE));
        // Posteriori error estimate covariance matrix
        cv::setIdentity(filter.errorCovPost, cv::Scalar::all(ESTIMATE_ERROR_VARIANCE));
}

KalmanFilter::~KalmanFilter() {

}

cv::Point KalmanFilter::predict() {
        // First predict, to update the internal statePre variable
        prediction = filter.predict();
        predictedPoint = cv::Point(prediction.at<float>(0), prediction.at<float>(1));

        // Ensure that filter doesnt get stuck if no corrections are made
        filter.statePre.copyTo(filter.statePost);
        filter.errorCovPre.copyTo(filter.errorCovPost);

        return predictedPoint;
}

cv::Point KalmanFilter::correct(float x, float y) {
        measurement(0) = x;
        measurement(1) = y;

        // Update with specified values
        estimation = filter.correct(measurement);
        estimatedPoint = cv::Point(estimation.at<float>(0), estimation.at<float>(1));
        measuredPoint = cv::Point(measurement(0), measurement(1));

        return estimatedPoint;
}