rimm and imm.m

RIMM与IMM的程序比较

function rimm_imm()               %交互式多模型  一维的情况(没有求出平均)
%对输入的x,p进行改动后的
%rimm rimm两者之间的比较
clear all;
close all;
timenum=100;
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;          %目标2在模型i在k-1时刻的概率
Q1=100;            %目标1的状态噪声协方差阵
Q2=400;           %目标2的状态噪声协方差阵
R=1000;           %观测噪声协方差阵
x00=[0 0];
p00=[10 0];
x00_old=[0 0];
p00_old=[10 0];
xx1=0;
xx2=0;
f1=1;
f2=1;
l1=t^2/t;
l2=t^2/t;
h=1;
I=1;
for k=1:timenum
    xx_1(k)=f1*xx1+l1*sqrt(Q1)*randn(1);
    xx_2(k)=f2*xx2+l2*sqrt(Q2)*randn(1);
    z_1(k)=h*xx_1(k)+sqrt(R)*randn(1);
    z_2(k)=h*xx_2(k)+sqrt(R)*randn(1);
    %================input alternation==============================
    tran_11=transfer_matric(1,1)*u_10;
    tran_12=transfer_matric(1,2)*u_10;
    tran_21=transfer_matric(2,1)*u_20;
    tran_22=transfer_matric(2,2)*u_20;
    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;
    m_ch(1,1)=(transfer_matric(1,1)/sum_tran_j1)*f1*p00(1)*f1'+Q1;
    m_ch(1,2)=(transfer_matric(1,2)/sum_tran_j2)*f1*p00(1)*f1'+Q1;
    m_ch(2,1)=(transfer_matric(2,1)/sum_tran_j1)*f2*p00(2)*f2'+Q2;
    m_ch(2,2)=(transfer_matric(2,2)/sum_tran_j2)*f2*p00(2)*f2'+Q2;
    for i=1:2
    m_inv_j(i)=u_mix(i,1)*inv(m_ch(i,1))+u_mix(i,2)*inv(m_ch(i,2));
    end;
    f=[f1 f2];
    for j=1:2
        for i=1:2
            s(j,i)=u_mix(j,i)*inv(m_inv_j(j))*inv(m_ch(j,i))*f(j);
        end;
    end;
            
  for i=1:2
    x00_estimate_j(i)=s(i,1)*x00(1)+s(i,2)*x00(2);
    p00_estimate_j(i)=s(i,1)*(p00(1)+(x00(1)-x00_estimate_j(i))*(x00(1)-x00_estimate_j(i))')+...
                     s(i,2)*(p00(2)+(x00(2)-x00_estimate_j(i))*(x00(2)-x00_estimate_j(i))') ;%p00_estimate以行存储
   end;
   
   for i=1:2
    x00_estimate_j_old(i)=u_mix(i,1)*x00_old(1)+u_mix(i,2)*x00_old(2);
    p00_estimate_j_old(i)=u_mix(i,1)*(p00_old(1)+(x00_old(1)-x00_estimate_j_old(i))*(x00_old(1)-x00_estimate_j_old(i))')+...
                      u_mix(i,2)*(p00_old(2)+(x00_old(2)-x00_estimate_j_old(i))*(x00_old(2)-x00_estimate_j_old(i))') ;%p00_estimate以行存储
   end;
   
   
  %==================filter calculate=================================
    %================model 1===================================
     x1_pre(k)=f1*x00_estimate_j(1);
     p1_pre(k)=f1*p00_estimate_j(1)*f1'+l1*Q1*l1';
     s1=h*p1_pre(k)*h'+R;
     k_gain1=p1_pre(k)*h'*inv(s1);
     p1_estimate(k)=(I-k_gain1*h)*p1_pre(k);
     v1=z_1(k)-h*x1_pre(k);
     x1_estimate(k)=x1_pre(k)+k_gain1*v1;
     
     x1_pre_old(k)=f1*x00_estimate_j_old(1);
     p1_pre_old(k)=f1*p00_estimate_j_old(1)*f1'+l1*Q1*l1';
     s1_old=h*p1_pre_old(k)*h'+R;
     k_gain1=p1_pre_old(k)*h'*inv(s1_old);
     p1_estimate_old(k)=(I-k_gain1*h)*p1_pre_old(k);
     v1_old=z_1(k)-h*x1_pre_old(k);
     x1_estimate_old(k)=x1_pre_old(k)+k_gain1*v1_old;
    %================model 2===================================
     x2_pre(k)=f2*x00_estimate_j(2);
     p2_pre(k)=f2*p00_estimate_j(2)*f2'+l2*Q2*l2';
     s2=h*p1_pre(k)*h'+R;
     k_gain2=p2_pre(k)*h'*inv(s2);
     p2_estimate(k)=(I-k_gain2*h)*p2_pre(k);
     v2=z_2(k)-h*x2_pre(k);
     x2_estimate(k)=x2_pre(k)+k_gain2*v2;
     
     x2_pre_old(k)=f2*x00_estimate_j_old(2);
     p2_pre_old(k)=f2*p00_estimate_j_old(2)*f2'+l2*Q2*l2';
     s2_old=h*p1_pre_old(k)*h'+R;
     k_gain2=p2_pre_old(k)*h'*inv(s2_old);
     p2_estimate_old(k)=(I-k_gain2*h)*p2_pre_old(k);
     v2_old=z_2(k)-h*x2_pre_old(k);
     x2_estimate_old(k)=x2_pre_old(k)+k_gain2*v2_old;
  %===============================================================
  %===================model probability alternate=================
  s=[s1 s2];
  s_old=[s1_old s2_old];
  v=[v1 v2];
  v_old=[v1_old v2_old];
  for i=1:2   
  model_fun(i)=(1/sqrt(2*pi*s(i)))*exp(-v(i)*v(i)'/(2*s(i)));
  model_fun_old(i)=(1/sqrt(2*pi*s_old(i)))*exp(-v_old(i)*v_old(i)'/(2*s_old(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];
  
   sum_tran_old=model_fun_old(1)*sum_tran_j1+model_fun_old(2)*sum_tran_j2;
  u_j1_old=model_fun_old(1)*sum_tran_j1/sum_tran;
  u_j2_old=model_fun_old(2)*sum_tran_j2/sum_tran;
  %u_j_old=[u_j1_old u_j2_old];
  %==============================================================
  %====================output alternation=========================
 kexi_inv=u_j1*inv(p1_estimate(k))+u_j2*inv(p2_estimate(k));
 p_j=[p1_estimate(k) p2_estimate(k)];
 for i=1:2
     fi(i)=u_j(i)*inv(kexi_inv)*inv(p_j(i));
 end;
  x_sum(k)=x1_estimate(k)*fi(1)+x2_estimate(k)*fi(2);
  p_sum(k)=(p1_estimate(k)+(x_sum(k)-x1_estimate(k))*(x_sum(k)-x1_estimate(k))')*u_j1+...
           (p2_estimate(k)+(x_sum(k)-x2_estimate(k))*(x_sum(k)-x2_estimate(k))')*u_j2;
       
  x_sum_old(k)=x1_estimate_old(k)*u_j1_old+x2_estimate_old(k)*u_j2_old;
  p_sum_old(k)=(p1_estimate_old(k)+(x_sum_old(k)-x1_estimate_old(k))*(x_sum_old(k)-x1_estimate_old(k))')*u_j1_old+...
           (p2_estimate_old(k)+(x_sum_old(k)-x2_estimate_old(k))*(x_sum_old(k)-x2_estimate_old(k))')*u_j2_old;
  %x00=x_sum(k);
  x00=[x1_estimate(k) x2_estimate(k)];
  x00_old=[x1_estimate_old(k) x2_estimate_old(k)];
  %p00=p_sum(k);
  p00=[p1_estimate(k) p2_estimate(k)];
  p00_old=[p1_estimate_old(k) p2_estimate_old(k)];
  xx1=xx_1(k);
  xx2=xx_2(k);
end;

figure(1);
plot(xx_1,'b');
hold on;
plot(xx_2,'m');
plot(x1_estimate_old,'-.b');
hold on;
plot(x2_estimate_old,'-.m');
plot(x_sum_old,'r')
hold off;
legend('模型1的状态','模型2的状态','模型1的估计','模型2的估计','融合输出的估计');
xlabel('采样次数');
ylabel('IMM情况下的相应值');
grid;

figure(2);
plot(xx_1,'b');
hold on;
plot(xx_2,'m');
plot(x1_estimate,'-.b');
hold on;
plot(x2_estimate,'-.m');
plot(x_sum,'r')
hold off;
legend('模型1的状态','模型2的状态','模型1的估计','模型2的估计','融合输出的估计');
xlabel('采样次数');
ylabel('RIMM情况下的相应值');
grid;

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