weight_fusion_exp.m

来自「加权融合算法」· M 代码 · 共 112 行

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