%Define the length of the simulation.
nlen=10;
nx=1;
nz=1;

% %Define the system.  Change these to change the system.
% a=1;  % a=1 for a constant, |a|<1 for a first order system.
% h=1;

%Define the noise covariances.
Q=.0;
R=0.1;

%Preallocate memory for all arrays. Note that this is not really necessary
%but speeds up MatLab a bit.
%However, it is necessary to come up with initial estimates (guesses) for
% 1) the state.
% 2) the a priori error.
X=zeros(nx,nlen);
z=zeros(nz,nlen);
% z=[psi kappa]';
% z=kappa_odo./psi_pot;

xapriori=zeros(nx,nlen);
xaposteriori=zeros(nx,nlen);
residual=zeros(nz,nlen);
papriori=ones(nx,nx);
paposteriori=ones(nx,nx);
k=zeros(nx,nz,nlen);

%Calculate the process and measurement noise.
w1=randn(nx,nlen); %This line and the next can be commented out after running
v1=randn(nz,nlen); %the script once to generate multiple runs with identical
%noise (for better comparison).
w=w1*sqrt(Q);
v=v1*sqrt(R);

%Initial condition on the state, x.
X_0=[0 ];

%Initial guesses for state and a posteriori covariance.
xaposteriori_0=[0];
paposteriori_0=eye(nx,nx);

%Calculate the first estimates for all values based upon the initial guess
%of the state and the a posteriori covariance. The rest of the steps will
%be calculated in a loop.
% Calculate the state and the output
   

%     a=eye(nx,nx);

a= 1;
   
h=1;
    
% X(:,1)=a*X_0+w(:,1);
% z(:,1)=h*X(:,1)+v(1);
z=[0.39 0.5 .48 .29 .25 .32 .34 .48 .41 .45];
%Predictor equations
xapriori(:,1)=a*xaposteriori_0;



residual(:,1)=z(:,1)-h*xapriori(:,1);
papriori=a*paposteriori_0*a'+Q;



%Corrector equations
k(:,:,1)=papriori*h'/(h*papriori*h'+R);
paposteriori=(1-k(:,:,1)*h)*papriori;
xaposteriori(:,1)=xapriori(:,1)+k(:,:,1)*residual(:,1);

%Calculate the rest of the values.
for j=2:nlen,
    
    
    
%     Calculate the state and the output
%     X(:,j)=a*X(:,j-1)+w(:,j);
%     z(:,j)=h*X(:,j)+v(j);
%     Predictor equations
    xapriori(:,j)=a*xaposteriori(:,j-1);
    
    
    residual(:,j)=z(:,j)-h*xapriori(:,j);
    papriori=a*paposteriori*a'+Q;
    %Corrector equations
    k(:,:,j)=papriori*h'/(h*papriori*h'+R);
    paposteriori=(eye(nx,nx)-k(:,:,j)*h)*papriori;
    xaposteriori(:,j)=xapriori(:,j)+k(:,:,j)*residual(:,j);
end

plot(xaposteriori(:,nlen))