% Tests estimator
clear all

xhat0=2.5;
n=20;


NL=200;
Ng=1;
ell=1;

time=1:NL; % Time steps during a lifetime
y=zeros(NL,Ng); % Initialize/allocate memory for sensed variable from environment
z=zeros(NL,Ng); % Initialize
sigmaz2=0.5*ones(NL,Ng);  % Initialize the variance in the variable we want to estimate (could
                      % change during a lifetime or over generation)

xbar=2; % For case where have an unknown constant in the environment that want to estimate
x=xbar*ones(NL,Ng); % Set variable want to estimate as a constant
xhat=zeros(NL,Ng);


% Set weighting parameters for cost function

w1=0.01;
w2=1; 
w3=.1;

clear Y  % Dimension changes on the regressor vector so clear it each time it changes
Y=xhat0*ones(n,1); % Set initial condition
	
	% Run estimator for lifetime of animal
	for k=1:NL  % Generate sensed signals, including initial values that are instincts
		z(k,ell)=sigmaz2(k,ell)*randn; % Generate noise
		y(k,ell)=x(k,ell)+z(k,ell); % Generate signal that is sensed
		Y=[y(k,ell); Y(1:(n-1),1)]; % Shift regression vector, load in new value
		xhat(k,ell)=mean(Y);
	end

J=w1*n+w2*(1/NL)*(x(:,ell)-xhat(:,ell))'*(x(:,ell)-xhat(:,ell))...
			-w3*(x(1,ell)-xhat0)^2

% Plot results

figure(1) 
clf
plot(time,x,'r.',time,y,'g--',time,xhat,'b-')
xlabel('Iteration')
title('x (dotted), y (dashed), xhat (solid)')

