#include <ctrlbase.h>

LQG::set_system_parameters(const mat &A0, const mat &B0, const mat &C0){
	dimx = A0.rows();
	dimy = C0.rows();
	dimu = B0.cols();

	it_assert_debug ( A0.cols() == dimx, "LQG: A is not square" );
	it_assert_debug ( B0.rows() == dimx, "LQG: B is not compatible" );
	it_assert_debug ( C0.cols() == dimx, "LQG: C is not square" );

	A=A0;
	B=B0;
	C=C0;
	
	// set temporary stuff;
	pr = concat_vertical( 
					concat_horizontal(B,A, zeros(dimx, dimu+dimy)),
					concat_horizontal(zeros(dimu+dimy,dimu+dimx), eye(dimu+dimy)));
	
	//penalization
	qux=zeros(dimu,dimx+2*dimu+dimy);     
	qux.set_submatrix(0,0,Qu);
	qux.set_submatrix(0,dimx+dimu+dimy,Qu);

	qyx=concat_horizontal(C,-eye(dimy),zeros(dimy,dimu));
	
	//
	s=1e-5*eye(dimx+dimu+dimy);
	// parts of QR
	hqy=Qy*qyx*pr;
	
	// pre_qr
	pre_qr = concat_vertical(s*pr, hqy, qux);
	
}