Ensemble Kalman Filter

syms x1 x2;%variables must be named x1...xn
f=[x1+.1*x2+.005;x2+.1];
h=[x1];
x_tr=[1;1];%initial value of state
x_ini=ones(2,20);%ensemble of initial estimate of the state
w=[10^-3;.02];%process noise standard deviation
z=[10];%measurement noise standard deviation
num_iterations=600;
num_members=20;
[dummy,num_members]=size(x_ini);
p1=length(f);
m1=length(h);
var_vector=[];
xvec=[];
x_estvec=[];
yvec=[];
x_est=x_ini;
for j=1:p1%createvector containing variables x1 to xn
    eval(sprintf(' syms x%d', j));
    var_vector=[var_vector sprintf('x%d ',j)];
end
var_vector=strcat('[',var_vector);
var_vector=strcat(var_vector,']');
Zcov=eye(m1);%create measurement noise covariance matrix
for j=1:m1
    Zcov(j,j)=z(j)^2;
end
for i=1:num_iterations
    
    x_tr=subs(f,var_vector,x_tr)+w.*randn(p1,1);
    %computetruevalueofstateatnexttimestep
    for j=1:num_members
        W(:,j)=w.*randn(p1,1);%createprocessnoise
        Z(:,j)=z.*randn(m1,1);%createmeasurementnoise
        x_est(:,j)=subs(f,var_vector,x_est(:,j))+W(:,j);%forecaststate
        y(:,j)=subs(h,var_vector,x_tr)+Z(:,j);%makemeasurement
        y_for(:,j)=subs(h,var_vector,x_est(:,j));%forecastmeasurement
    end
    x_estbar=mean(x_est,2);
    ybar=mean(y,2);
    y_forbar=mean(y_for,2);
    for j=1:p1
        Ex(j,:)=[x_est(j,:)-x_estbar(j)];
    end
    for j=1:m1
        Ey(j,:)=[y_for(j,:)-y_forbar(j)];
    end
    Pxy=Ex*Ey'/(num_members-1);
    Pyy=Ey*Ey'/(num_members-1)+Zcov;
    K=Pxy*inv(Pyy);
    x_est=x_est+K*(y-y_for);
    xvec=[xvecx_tr];
    x_estvec=[x_estvecx_estbar];
    yvec=[yvecybar];
    if i==num_iterations
        x_estbar=mean(x_est,2);
    end
end