kalman2.m

学习过程中

%分布式卡尔曼滤波程序
clear;
N=5;Fai=0.98;Q=0.1;
H1=0.92;H2=0.92;H3=0.92;H4=0.92;H5=0.92;
R1=0.1;R2=0.1;R3=0.1;R4=0.1;R5=0.1;
x0=0;p0=0;I=1;

xtk=[];
ptk=[];
for k=1:100
    v = sqrt(0.1) * randn(1);            %观测噪声 v(k) 
    
%一步预测估计值和相应的预测误差协方差阵
x(k)=Fai*x0;
p(k)=Fai*p0*Fai'+Q;

%用z1k对x1k进行更新
x1(k)=x(k);p1(k)=p(k);
z1(k)=H1*x(k)+v;
k1=p1(k)*H1'*inv(H1*p1(k)*H1'+R1);
x1y(k)=Fai*x0+k1*(z1(k)-H1*x1(k));
p1y(k)=(I-k1*H1)*p1(k);

%用z2(k)对x1y(k)进行更新
x2(k)=x1y(k);p2(k)=p1y(k);
z2(k)=H2*x1y(k)+v;
k2=p2(k)*H2'*inv(H2*p2(k)*H2'+R2);
x1t=k1*(z1(k)-H1*x1(k))+k2*(z2(k)-H2*x2(k));
p1t=(I-k2*H2)*(I-k1*H1);
x2y(k)=Fai*x0+x1t;
p2y(k)=p1t*p1(k);

%N=3
x3(k)=x2y(k);p3(k)=p2y(k);
z3(k)=H3*x2y(k)+v;
k3=p3(k)*H3'*inv(H3*p3(k)*H3'+R3);
x2t=x1t+k3*(z3(k)-H3*x3(k));
p2t=(I-k3*H3)*p1t;
x3y(k)=Fai*x0+x2t;
p3y(k)=p2t*p1(k);

%N=4 
x4(k)=x3y(k);p4(k)=p3y(k);
z4(k)=H4*x3y(k)+v;
k4=p4(k)*H4'*inv(H4*p4(k)*H4'+R4);
x3t=x2t+k4*(z4(k)-H4*x4(k));
p3t=(I-k4*H4)*p2t;
x4y(k)=Fai*x0+x3t;
p4y(k)=p3t*p1(k);

%N=5 
x5(k)=x4y(k);p5(k)=p4y(k);
z5(k)=H5*x4y(k)+v;
k5=p5(k)*H5'*inv(H5*p5(k)*H5'+R5);
x4t=x3t+k5*(z5(k)-H5*x5(k));
p4t=(I-k5*H5)*p3t;
x5y(k)=Fai*x0+x4t;
p5y(k)=p4t*p1(k);

xtk=[xtk x5y(k)];
ptk=[ptk p5y(k)];

x(k)=x5y(k);
p(k)=p5y(k);
end


%图形
figure
plot(xtk(:),'b-')
title('CDFA')