分布式kalman滤波matlab代码

%DKF ·Ö²¼Ê½kalmanÂ˲¨

close all

clear

clc

%³õʼ»¯

sNUM=25; %´«¸ÐÆ÷Êý

T=0.015; %²ÉÑùÖÜÆÚ

N=100; %ÿ¸ö´«¸ÐÆ÷²ÉÑùµãÊý

A0=2*[0,-1;1,0];

B0=5^2*eye(2);

A=eye(2)+T*A0+T^2/2*A0^2+T^3/6*A0;

B=T*B0;

H=[1,0;0,1];

%´«¸ÐÆ÷״̬³õÖµ

x0=[15;-10];

P0=10*eye(2);

for i=1:sNUM

x(:,1,i)=x0;

xg(:,1)=[0,0];

P(:,:,i)=P0;

end

err(:,1)=sum(x(:,1,:),3)/sNUM-xg(:,1);

Ex(:,1)=[P0(1,1);P0(2,2)];

%ÔëÉù

Q=10;

cv=10;

for i=1:sNUM

R(:,:,i)=cv^2*sqrt(i)*eye(2);

w(:,:,i)=sqrt(Q)*randn(2,N);

v(:,:,i)=sqrt(R(:,:,i))*randn(2,N);

end

%¹¹Ôì¹Û²âÍøÂç

T=connectionMb(sNUM); %¹Û²âÍøÂçÁ¬½Ó¾ØÕó

D=diag(sum(T,2)); %¶È¾ØÕó

L=D-T; %À-ÆÕÀ-˹¾ØÕó

%¹¹Ôì¹Û²âÖµ

for i=1:sNUM

for k=2:N

x(:,k,i)=A*x(:,k-1,i)+B*w(:,k,i);

z(:,k,i)=H*x(:,k,i)+v(:,k,i);

%˳±ã¼ÆËãÐÅÏ¢Á¿

u(:,k,i)=H'*inv(R(:,:,i))*z(:,k,i);

end

U(:,:,i)=H'*inv(R(:,:,i))*H;

end

%Â˲¨Ëã·¨

for k=2:N

for i=1:sNUM

T1=T+eye(sNUM);

% y(:,k,i)=reshape(u(:,k,:),2,sNUM)*T(:,i);

y=[0;0];

S=zeros(2);

for j=1:sNUM

y=y+T1(j,i)*u(:,k,j);

S=S+T1(j,i)* U(:,:,j);

end

M(:,:,i)=inv(inv(P(:,:,i))+S);

f(:,i)=xg(:,k-1)+M(:,:,i)*(y-S*xg(:,k-1));

P(:,:,i) =A*M(:,:,i)*A'+B*Q*B';

end

xk(1,k)=consencous(f(1,:)',L);

xk(2,k)=consencous(f(2,:)',L);

xg(:,k)=A*xk(:,k);

err(:,k)=sum(x(:,k,:),3)/sNUM-xg(:,k);

Ex(:,k)=[P(1,1,1);P(2,2,1)];

end

figure

subplot(2,1,1)

plot(err(1,:))

title('״̬Îó²îÇúÏß')

subplot(2,1,2)

plot(err(2,:))

figure

subplot(2,1,1)

plot(Ex(1,:))

title('״̬·½²îÇúÏß') subplot(2,1,2)

plot(Ex(2,:))