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