function testKF(skipgen)
if nargin<1, skipgen=0; end

if ~skipgen
	A=[1 -0.5; 1 0];
	B=[1;0.1];
	C=[1 0; 0 1];
	D=[0.1; 0];
	R=[1 0; 0 0.1];
	Q=[0.2 0 ; 0 0.2]; 

	sQ = chol(Q)';
	sR = chol(R)';

	N =1000;
	mu0 = [0;0];
	P0 = 200*eye(2);

	u = rand(1,N);
	x = zeros(2,N);
	y = zeros(2,N);

	x(:,1) = [10;10];
	Et = sQ*randn(2,N);
	Wt = sR*randn(2,N);
	for i=2:N;
		x(:,i) = A*x(:,i-1) + B*u(i) + Et(:,i);
		y(:,i) = C*x(:,i) + D*u(i) + Wt(:,1);
	end

	d=[y;u];
	itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0)
	save testKF
else
	load testKF
end
% init 
mu = mu0;
P = P0;
EP = [0;0];

Oxt = mu0;
OPt = P0;
ll =0;
Oxt2 = mu0;
OPt2 = P0;
ll2=0;


for t=2:N
	mu = A*mu + B*u(t);
	P  = A*P*A' + Q;

	%Data update
	Ry = C*P*C' + R;
	iRy = inv(Ry);
	K = P*C'*iRy; 
	P = P- K*C*P; % P = P -KCP;
	mu = mu + K*(y(t)-C*mu-D*u(t));
	Mu(1:2,t)=mu;

	[Oxt,OPt,ll(t)] = Kalman(Oxt,y(t),A,C,Q,R,OPt);
	[Oxt2,OPt2,ll2(t)] = Kalman(Oxt2,y(t),A,C,Q,R/10000,OPt2);
end

keyboard

!cd ../;./tests/testKF
itload('testKF_res.it');

hold off
plot(x');
hold on
plot(xth','--');
plot(xth2','+');
plot(xthE','o');
plot(Mu',':');
keyboard
end