交互多模型卡尔曼滤波

  • locust_dpw
    了解作者
  • matlab
    开发工具
  • 1.7KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 0
    下载次数
  • 2022-08-14 20:39
    上传日期
交互多模型卡尔曼滤波算法,供初学者学习改进。
IMM.rar
  • IMM.m
    5.5KB
内容介绍
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
评论
    相关推荐