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
Ensemble Kalman Filter
于 2020-03-03 14:30:51 首次发布