clear all;

modelresolution=40;
ensemblesize=10;
forcing=8.;
timestep=0.05;
maxtime=51000;
initial_model_perturbation=0.008;
initial_ensemble_perturbation=0.01;
truth=ones(modelresolution,1)*forcing;
ensemble=ones(modelresolution,ensemblesize)*forcing;

obs_percent=50;
number_observations=round(modelresolution*(obs_percent/100));
observation_error=0.5;
observations=zeros(number_observations,1);
plot_observations=zeros(modelresolution,1);
R=zeros(number_observations,number_observations);
I=eye(modelresolution);
H=zeros(number_observations,modelresolution);

count=0;
for j=1:modelresolution
  if mod(j,(obs_percent/100)^-1) == 0
     count=count+1;
     R(count,count)=observation_error;
     H(count,:)=I(j,:);
  end
end

xb_bar=zeros(modelresolution,1);
yb_bar=zeros(number_observations,1);
Xb=zeros(modelresolution,ensemblesize);
Yb=zeros(number_observations,ensemblesize);
C=zeros(ensemblesize,number_observations);
rho=1.01;  
Pa_bar=zeros(ensemblesize,ensemblesize); 
Wa=zeros(ensemblesize,ensemblesize);

lorenz_96_initialize

for t=1:maxtime
    
   count=0;
   for j=1:modelresolution
     if mod(j,(obs_percent/100)^-1) == 0
        count=count+1;
        plot_observations(j)=truth(j)+observation_error*randn(1);
        observations(count)=truth(j)+observation_error*randn(1);
     else
        plot_observations(j)=NaN;
     end
   end
   
   for j=1:modelresolution
     xb_bar(j)=mean(ensemble(j,:));
     for i=1:ensemblesize
       Xb(j,i)=ensemble(j,i)-xb_bar(j);
     end
   end
   yb_bar=H*xb_bar;
   Yb=H*Xb;

   C=Yb'*R^-1;
   
   Pa_bar=(((ensemblesize-1)*eye(ensemblesize))/rho+C*Yb)^-1;
   
   Wa=((ensemblesize-1)*Pa_bar)^0.5;
   
   wa_bar=Pa_bar*C*(observations-yb_bar);
   for j=1:ensemblesize
     wai(:,j)=Wa(:,j)+wa_bar; 
   end

   temp=Xb*wai;
   for j=1:ensemblesize
     xai(:,j)=xb_bar+temp(:,j);
   end
   
   ensemble(:,:)=xai(:,:);
   
   figure(1);
   plot(truth(:),'k-');
   hold on
   for j=1:ensemblesize
     plot(ensemble(:,j),'b--');
   end
   axis([1 40 -15 15]);
   plot(plot_observations(:),'r*');
   hold off
   
   lorenz_96_cycle
   
   pause
   
end