/*
 * KalmanWrapper.hpp
 *
 *  Created on: Apr 3, 2020
 *      Author: lukas
 */

#ifndef SRC_KALMANWRAPPER_HPP_
#define SRC_KALMANWRAPPER_HPP_

#include "opencv2/opencv.hpp"

using namespace cv;
using namespace std;

class KalmanWrapper {
public:
	KalmanWrapper(String model_type) ;
	virtual ~KalmanWrapper();

	void setQ(Mat mat) { _kf.processNoiseCov = mat; };
	void setP(Mat mat) { _kf.errorCovPost = mat; };
	void setR(Mat mat) { _kf.measurementNoiseCov = mat; };
	void setInitialState(Mat mat) { _kf.statePost = mat; };

	void setInitialXY(Mat mat);
	void setParams(string path);
	void generatePrediction(Mat &prediction);
	void generateCorrection(Mat measurement, Mat &corrected_measurement);

private:
	KalmanFilter _kf;
	string _model_type;

	int _state_size;
	int _meas_size;

	Mat _A; // Transition matrix
	Mat _H; // Measurement matrix (state -> obs)
	Mat _P; // Covariance matrix
	Mat _Q; // Process noise
	Mat _R; // Noise of the observation
	Mat _default_P; // default covariance matrix (from the lecture)
	Mat _default_Q; // default process noise (from the lecture)
	Mat _default_R; // default observation noise (from the lecture)

};

#endif /* SRC_KALMANWRAPPER_HPP_ */