#include "estim/kalman.h"
using namespace bdm;

// estimation of AR(0) model
int main() {
	//dimensions
	int dx = 3, dy = 3, du = 1;
	// matrices
	mat A = eye ( dx );
	mat B = zeros ( dx, du );
	mat C = eye ( dx );
	mat D = zeros ( dy, du );
	mat Q = eye ( dx );
	mat R = 0.1 * eye ( dy );
	//prior
	mat P0 = 100 * eye ( dx );
	vec mu0 = zeros ( dx );
	// Estimator
	KalmanCh KF;
	KF.set_parameters ( A, B, C, D,/*covariances*/ Q, R );
	KF.set_statistics ( mu0, P0 );
	KF.validate();
	// Estimation loop
	for ( int i = 0; i < 100; i++ ) {
		KF.bayes ( randn ( dy), randn( du ) );
	}
	//print results
	cout << "Posterior estimate of x is: "  << endl;
	cout << "mean: " << KF.posterior().mean() << endl;
	cout << "variance: " << KF.posterior().variance() << endl;
}
