jiaohushiduomoxing.m

本程序为交互式多模型算法

function imm_extend2()               %交互式多模型  初步改进
clear all;
close all;
timenum=75;
t=1;
transfer_matric=[0.98  0.02;  %马尔科夫转移矩阵 矩阵为j行i列
                 0.02  0.98];
u_10=0.5;          %目标1在模型i在k-1时刻的概率
u_20=0.5;          %目标1在模型i在k-1时刻的概率
u=[u_10 u_20];
Q1=100;            %目标1的状态噪声协方差阵
Q2=4000;           %目标1的状态噪声协方差阵
R=10000;           %观测噪声协方差阵
%R2=700;
x00_1=[0 0 0]';            %模型1估计的初始值
x00_2=[0 0 0]';            %模型2估计的初始值
p00_1=[100   0    0;
    0     10   0;
    0       0  0];
p00_2=[100   0    0;
    0     10   0;
    0       0  10];
dis=1000; %位移
vel=450;  %速度
acc=50;  %加速度
xx01=[dis vel 0]';            %状态的初始值
xx02=[dis vel acc]';            %状态的初始值
%xx2=0;
f1=[1 t 0;
    0 1 0;
    0 0 0];             %状态转移阵
f2=[1 t 0.5*t^2;
    0 1 t;
    0 0 1];
l1=[0.5*t^2 t 0]';         %状态干扰阵
l2=[0.5*t^2 t 1]';
h=[1 0 0];              %观测转移阵
I=[1 0 0;
    0 1 0;
    0 0 1];
num=1;
XX1=zeros(3,25);
XX2=XX1;
XX3=XX1;
while num<=timenum
    if num<=25
       xx1=f1*xx01+l1*sqrt(Q1);  %目标的状态值
        XX1(:,num)=xx1;
          z(num)=h*xx1+sqrt(R)*randn(1);    %目标的观测值    
     else 
          if 25<num<=50
          xx2=f2*xx02+l2*sqrt(Q2);  %目标的状态值
            XX2(:,num-25)=xx2;
           z(num)=h*xx2+sqrt(R)*randn(1);    %目标的观测值  
         else 
             if 50<num<=timenum
            xx3=f1*xx01+l1*sqrt(Q1);  %目标的状态值
            XX3(:,num-50)=xx3;
           z(num)=h*xx3+sqrt(R)*randn(1);    %目标的观测值    
           end;
         end;
     end;
    
    %================input alternation==============================
    tran_11=transfer_matric(1,1)*u(1);
    tran_12=transfer_matric(1,2)*u(1);
    tran_21=transfer_matric(2,1)*u(2);
    tran_22=transfer_matric(2,2)*u(2);
    sum_tran_j1=tran_11+tran_21;
    sum_tran_j2=tran_12+tran_22;
    sum_tran_j=[sum_tran_j1 sum_tran_j2];
    u_mix(1,1)=tran_11/sum_tran_j1;
    u_mix(1,2)=tran_12/sum_tran_j2;
    u_mix(2,1)=tran_21/sum_tran_j1;
    u_mix(2,2)=tran_22/sum_tran_j2;
   % for i=1:2
   % x00_estimate_j(i)=sum_tran_j(i)*x00(i);
   % p00_estimate_j(i)=sum_tran_j(i)*(p00+(x00-x00_estimate_j(i))*(x00-x00_estimate_j(i))');%p00_estimate以行存储
   %end;
   x00_estimate_j(:,1)=x00_1*u_mix(1,1)+x00_2*u_mix(2,1);
   x00_estimate_j(:,2)=x00_1*u_mix(1,2)+x00_2*u_mix(2,2);
   p00_estimate_j1=(p00_1+(x00_1-x00_estimate_j(:,1))*(x00_1-x00_estimate_j(:,1))')*u_mix(1,1)+...
                            (p00_2+(x00_2-x00_estimate_j(:,1))*(x00_2-x00_estimate_j(:,1))')*u_mix(2,1);
   p00_estimate_j2=(p00_1+(x00_1-x00_estimate_j(:,2))*(x00_1-x00_estimate_j(:,2))')*u_mix(1,2)+...
                            (p00_2+(x00_2-x00_estimate_j(:,2))*(x00_2-x00_estimate_j(:,2))')*u_mix(2,2); 
  %==================filter calculate=================================
  %================model 1===================================
     x1_pre=f1*x00_estimate_j(:,1);
     p1_pre=f1*p00_estimate_j1*f1'+l1*Q1*l1';
     s1=h*p1_pre*h'+R;
     k_gain1=p1_pre*h'*inv(s1);
     p1_estimate=(I-k_gain1*h)*p1_pre;
     v1=z(num)-h*x1_pre;
     x1_estimate=x1_pre+k_gain1*v1;
     X1_estimate(:,num)=x1_estimate;
    %================model 2===================================
     x2_pre=f2*x00_estimate_j(:,2);
     p2_pre=f2*p00_estimate_j2*f2'+l2*Q2*l2';
     s2=h*p2_pre*h'+R;
     k_gain2=p2_pre*h'*inv(s2);
     p2_estimate=(I-k_gain2*h)*p2_pre;
     v2=z(num)-h*x2_pre;
     x2_estimate=x2_pre+k_gain2*v2;
     X2_estimate(:,num)=x2_estimate;
  %===============================================================
  %===================model probability alternate=================
  p_pre=[p1_pre;p2_pre];
  x_pre=[x1_pre' x2_pre'];
  s=[s1(1) s2(1)];
  v=[v1 v2];
  for i=1:2   
  model_fun(i)=(1/sqrt(2*pi*s(i)))*exp(-v(i)*v(i)'/(2*abs(s(i))));
  end;
  sum_tran=model_fun(1)*sum_tran_j1+model_fun(2)*sum_tran_j2;
  u_j1=model_fun(1)*sum_tran_j1/sum_tran;
  u_j2=model_fun(2)*sum_tran_j2/sum_tran;
  u_j=[u_j1 u_j2];
  %==============================================================
  %====================output alternation=========================
  x_sum=x1_estimate*u_j(1)+x2_estimate*u_j(2)

  X_sum(:,num)=x_sum;
  p_sum=(p1_estimate+(x_sum-x1_estimate)*(x_sum-x1_estimate)')*u_j1+(p2_estimate+(x_sum-x2_estimate)*(x_sum-x2_estimate)')*u_j2;
  P_sum(3*num-2:3*num,1:3)=p_sum;
  %x00=x_sum(k);
  %x00=[x1_estimate(k) x2_estimate(k)];
  x00_1=x1_estimate;
  x00_2=x2_estimate;
  %p00=[p1_estimate(k) p2_estimate(k)];
  p00_1=p1_estimate;
  p00_2=p2_estimate;

  
    if (num<=25|25<num<=75)
        xx01=xx1;
    else if 25<num<=50
        xx02=xx2;
        %else
       % xx01=xx3;
        end;
    end;
  
  %xx2=xx_2(k);
  u=[u_j1 u_j2];
  num=num+1;
end;
XX=[XX1 XX2 XX3];
figure;
plot(XX(1,:)','b');
hold on;
plot(X_sum(1,:),'m')
legend('状态','融合输出的估计');
plot(z,'r')

xlabel('采样次数');
ylabel('相应值');
grid;
hold off;
%figure;
%plot(p1_estimate,'b');
%hold on;
%plot(p2_estimate,'m');
%plot(p_sum,'r');
%hold off;
%legend('模型1的协方差','模型2的协方差','融合输出的协方差');
%xlabel('采样次数');
%ylabel('相应值');
%grid;

figure;
plot(XX(2,:)','b');
hold on;
plot(X_sum(2,:)','r');
legend('状态','融合输出的估计');
xlabel('采样次数');
ylabel('相应值');
grid;
hold off;

figure;
plot(XX(3,:),'b');
hold on;
plot(X_sum(3,:)','r');
legend('状态','融合输出的估计');
xlabel('采样次数');
ylabel('相应值');
grid;
hold off;
X_sum
XX