www.pudn.com > weight_fusion_exp.rar > weight_fusion_exp.m, change:2003-07-21,size:3206b

```function weight_fusion_exp(n)  %一个目标，多个传感器
a=0.9;
l=1;
h=[1 0 0]';
q=[0.01];
r=[0.010 0 0;
0  0.006 0;
0 0    0.007];
x0=0;
x00=0;
p00=100;
I=[1];
k=1;
while k<=n                                     %仿真时间
w=sqrt(q)*randn(1);                          %噪声v
v1=sqrt(r(1,1))*randn(1);                    %噪声w
v2=sqrt(r(2,2))*randn(1);
v3=sqrt(r(3,3))*randn(1);
v=[v1 v2 v3]';
x_state(k)=a*x0+l*w;                            %k时刻的状态方程
z=h*x_state(k)+v;                               %k时刻的观测方程
Z_gc(:,k)=z;
x_pre=a*x00;                                    %x在k状态下的预测
X_pre(:,k)=x_pre;
p_pre=a*p00*a'+l*q*l';                           %x在k状态下的预测误差协方差
K=p_pre*h'*inv(h*p_pre*h'+r);                      %卡尔曼增益矩阵
x_estimate=a*x00+K*(z-h*a*x00);                   %x(k|k)时刻的估计值
X_estimate(:,k)=x_estimate;
p_estimate=(I-K*h)*p_pre;                               %x(k|k)时刻估计误差的方差
P_estimate(:,2*k-1:2*k)=p_estimate;
z_pre(:,k)=h*a*x00;
sum=0;
for j=1:3
sum=sum+z_pre(j,k);
end;
z_sum(k)=sum;
x0=x_state(k);                                  %下一次开始的所已知的状态值
x00=x_estimate;                                      %x在下一次状态前的预测
p00=p_estimate;                                      %x在下一次状态前的误差协方差
k=k+1;
end;

i=1;
while i<=n
xweight=0;
xnoweight=0;
e_estimate=0;
for k=1:3
if z_sum(i)==0
weight(k,i)=0;
else
weight(k,i)=z_pre(k,i)/z_sum(i);
end;
e_estimate=e_estimate+weight(k,i)*X_pre(i);
xweight=xweight+weight(k,i)*X_estimate(i);
xnoweight=xnoweight+X_estimate(i);
end;
pweight=0;
pnoweight=0;
for k=1:3
pweight=pweight+weight(k,i)*(X_pre(i)-e_estimate)*(X_pre(i)-e_estimate)';
pnoweight=pnoweight+P_estimate(:,2*i-1:2*i);
end;
x_weight(i)=xweight;
x_no_weight(i)=xnoweight/3;
p_weight(i)=pweight;
p_no_weight(:,2*i-1:2*i)=pnoweight/3;
i=i+1;
end;

figure;
plot(x_state','r');
hold on;
plot(Z_gc','g');
plot(X_pre','b');
plot(X_estimate','m');
legend('采样点的状态值','采样点的观测值1','采样点的观测值2','采样点的预测值','采样点的估计值');
grid;
hold off;

figure;
plot(x_state','r');
hold on;
plot(x_weight,'m-.');
plot(x_no_weight,'b--');
grid;
legend('采样点的状态值','采样点在加权下的值','采样点在不加权下的值');
hold off;

figure;
k=1;
for i=1:n
P_es_single(k)=P_estimate(:,2*i-1);
P_es_double(k)=P_estimate(:,2*i)
P_no_single(k)=p_no_weight(:,2*i-1);
P_no_double(k)=p_no_weight(:,2*i);
k=k+1;
end;
plot(P_es_single,'ro');
hold on;
plot(P_es_double,'ro');
plot(p_weight','b');
plot(P_no_single,'g*');
plot(P_no_double,'g*');
legend('采样点的协方差值1','采样点的协方差值2','采样点在加权下的协方差值','采样点在不加权下的协方差值1','采样点在不加权下的协方差值2');
grid;
hold off;
a=P_estimate;
b=p_weight;
c=P_no_single;
a=x_weight
b=x_no_weight```