交互多模型容积卡尔曼

  • locust_dpw
    了解作者
  • matlab
    开发工具
  • 5.3KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 1
    下载次数
  • 2022-08-14 20:53
    上传日期
简单的交互多模型容积卡尔曼滤波,模型采用匀速和匀速转弯。
IMM-SRCKF.rar
  • IMM-SRCKF
  • CREAT_CTF.m
    223B
  • Untitled.m
    979B
  • IMM_SRCKF.m
    6KB
  • CREAT_CTT.m
    125B
  • IMM_SRCKF.asv
    6KB
内容介绍
clear all; clc; T=5; x0 = [20000,-270,35000,290,0]'; xA = []; zA = zeros(2,1); R=[100,0; 0,0.01]; %mode-1 匀速运动 A1=[1 T 0 0 0; 0 1 0 0 0; 0 0 1 T 0; 0 0 0 1 0; 0 0 0 0 1 ]; G1=[T^2/2 0 0; T 0 0; 0 T^2/2 0; 0 T 0; 0 0 0]; Q1=[0.1,0,0; 0,0.1,0; 0,0,0]; %mode-2 匀速转弯运动 A2=CREAT_CTF(pi/180,T); G2=CREAT_CTT(T); Q2=[0.1,0,0; 0,0.1,0; 0,0,1e-6]; X0=20000; Y0=20000; %观测站初始位置 %产生真实数据 x=x0; for k=1:26 x=A1*x+G1*sqrt(Q1)*[randn,randn,randn]'; xA =[xA x]; end for i=1:5 zA(:,i) =[((xA(1,i)-x0(1))^2+(xA(3,i)-x0(3))^2)^(-0.5);atan(xA(3,i)-x0(3))/(xA(1,i)-x0(1))]+[10 0;0 0.1]*[randn,randn]'; end %调用容积卡尔曼滤波算法 %采用两个模型,一为CV模型,二为CT模型 n=5;%系统维数 m=10;%容积点数 w=1/m;%权值 kesi=[1,0,0,0,0;0,1,0,0,0;0,0,1,0,0;0,0,0,1,0;0,0,0,0,1;-1,0,0,0,0;0,-1,0,0,0;0,0,-1,0,0;0,0,0,-1,0;0,0,0,0,-1]'; N=100;%%%采样点数 MN=100;%%%蒙特卡洛仿真次数 %{ r1=18000; a1=atan(-1.5); A=[cos(a1),-r1*cos(a1);sin(a1),r1*cos(a1)]; r=A*[100,0;0,0.01]*A'; P0=[r(1,1) r(1,1)/T r(1,2) r(1,2)/T 0; r(1,1)/T 2*r(1,1)/T^2 r(1,2)/T 2*r(1,2)/T^2 0; r(1,2) r(1,2)/T r(2,2) r(2,2)/T^2 0; r(1,2)/T 2*r(1,2)/T^2 r(2,2)/T 2*r(2,2)/T^2 0; 0 0 0 0 1]; %} P0=[100 0 0 0 0;0 1 0 0 0;0 0 100 0 0;0 0 0 1 0;0 0 0 0 1]; X1_k_1=x0; X2_k_1=x0; %每个模型的状态传播参数 P1=P0; P2=P0; %每个模型的状态传播参数 for mn= 1:100 Pi =[0.98,0.02;0.02,0.98];%%%%转移概率 u1=0.5; u2=0.5; %混合概率 c1=Pi(1,1)*u1+Pi(2,1)*u2; c2=Pi(1,2)*u1+Pi(2,2)*u2; u11=Pi(1,1)*u1/c1; u12=Pi(1,2)*u1/c2; u21=Pi(2,1)*u2/c1; u22=Pi(2,2)*u2/c2; x1_m = X1_k_1*u11+X2_k_1*u21; %初始状态交互/混合计算 x2_m = X1_k_1*u12+X2_k_1*u22; p1_k_1=(P1+(X1_k_1-x1_m)*(X1_k_1-x1_m)')*u11+(P2+(X2_k_1-x1_m)*(X2_k_1-x1_m)')*u21; p2_k_1=(P1+(X1_k_1-x2_m)*(X1_k_1-x2_m)')*u12+(P2+(X2_k_1-x2_m)*(X2_k_1-x2_m)')*u22; %%%%%ckf滤波 %%%%%时间更新 for n=1:100 %%%%%(1)求协方差矩阵平方根 Spost_1=chol(P1); Spost_2=chol(P2); Spost_1 =Spost_1'; Spost_2 =Spost_2'; %%%%%(2)计算求容积点 rjpoint1=Spost_1*kesi+ repmat(x1_m,1,10); rjpoint2=Spost_2*kesi+ repmat(x2_m,1,10); %%%%%(3)传播求容积点 Xminus1=A1* rjpoint1; %%%%容积点经过线性函数后的值 Xminus2=A2* rjpoint2; %%%%容积点经过非线性函数后的值 %%%(4)状态预测 for k=1:10 xminus_1=zeros(5,1); xminus_2=zeros(5,1); xminus_1=xminus_1+0.1*Xminus1(:,k); xminus_2=xminus_2+0.1*Xminus1(:,k); end %%%%(5)状态预测误差协方差阵的平方根 Pminus1= (Xminus1-repmat(xminus_1,1,10))/sqrt(2*n); Pminus2= (Xminus2-repmat(xminus_2,1,10))/sqrt(2*n); %%%%观测更新 %%%%%(1)矩阵分解 [foo1 Sminus1]=qr([Pminus1 sqrt(G1*Q1*G1')]',0); [foo2 Sminus2]=qr([Pminus2 sqrt(G2*Q2*G2')]',0); Sminus1=Sminus1'; Sminus2=Sminus2'; %%%%%(2)计算求容积点 rjpoint_1=Sminus1*kesi+repmat(xminus_1,1,10); rjpoint_2=Sminus2*kesi+repmat(xminus_2,1,10); %%%%(3)传播求容积点 Z_1=[sqrt((rjpoint_1(1,:)-repmat(X0,1,10)).^2+(rjpoint_1(3,:)-repmat(Y0,1,10)).^2);atan((rjpoint_1(3,:)-repmat(Y0,1,10))./(rjpoint_1(1,:)-repmat(X0,1,10)))]; Z_2=[sqrt((rjpoint_2(1,:)-repmat(X0,1,10)).^2+(rjpoint_2(3,:)-repmat(Y0,1,10)).^2);atan((rjpoint_2(3,:)-repmat(Y0,1,10))./(rjpoint_2(1,:)-repmat(X0,1,10)))]; for k=1:10 zhat_1=zeros(2,1); zhat_2=zeros(2,1); zhat_1=zhat_1+0.1* Z_1(:,k); zhat_2=zhat_1+0.1* Z_2(:,k); end %%%%(5)计算新息协方差矩阵的平方根 z1=(Z_1-repmat(zhat_1,1,10))/sqrt(2*n); z2=(Z_2-repmat(zhat_2,1,10))/sqrt(2*n); %%%%(6)计算互协方差矩阵的平方根 X1= (rjpoint_1-repmat(xminus_1,1,10))/sqrt(2*n); X2= (rjpoint_2-repmat(xminus_1,1,10))/sqrt(2*n); Pxzminus_1=X1*z1'; Pxzminus_2=X2*z2'; [foo_1 Sminus_1]=qr([z1 sqrt(R)]',0); [foo_2 Sminus_2]=qr([z2 sqrt(R)]',0); Sminus_1=Sminus_1'; Sminus_2=Sminus_2'; %%%%(7)计算卡尔曼增益 K1=Pxzminus_1*inv( Sminus_1')*inv(Sminus_1); K2=Pxzminus_2*inv( Sminus_2')*inv(Sminus_2); %%%%(8)状态更新 z=zA(:,k); xpre_1=xminus_1+K1*(z-zhat_1); xpre_2=xminus_2+K2*(z-zhat_2); %%%%(9)状态协方差矩阵更新 [foo_1_1 Sminus_1_1]=qr([X1-K1*z1 K1*sqrt(R)]',0); [foo_2_2 Sminus_2_2]=qr([X2-K2*z2 K2*sqrt(R)]',0); Sminus_1_1=Sminus_1_1'; Sminus_2_2=Sminus_2_2'; %%%%%%预测残差及协方差计算 v1=z-zhat_1; v2=z-zhat_2; sv1=Sminus_1*Sminus_1'; sv2=Sminus_2*Sminus_2'; %%%%%似然函数 a1=1/sqrt((2*pi)*det(sv1))*exp(-0.5*v1'*inv(sv1)*v1); a2=1/sqrt((2*pi)*det(sv2))*exp(-0.5*v2'*inv(sv2)*v2); %%%%模型概率更新 C=a1*c1+a2*c2; u1=a1*c1/C; u2=a2*c2/C; %估计融合 xk=xpre_1*u1+xpre_2*u2; %迭代 X1_k_1= xpre_1; X2_k_1= xpre_2; X_imm(:,n)=xk; um1(n)=u1; um2(n)=u2; end %%%%结束一次滤波 ex3(mn,:)=X_imm(1,:)-xA(1,:); %第mn次仿真x方向滤波误差 ey3(mn,:)=X_imm(3,:)-xA(3,:); %第mn次仿真y方向滤波误差 vx3(mn,:)=X_imm(2,:)-xA(2,:); %第mn次仿真x方向速度误差 vy3(mn,:)=X_imm(4,:)-xA(4,:); %第mn次仿真y方向速度误差 UM1(mn,:)=um1; UM2(mn,:)=um2; end %%%结束100次蒙特卡洛仿真 % EX1=sqrt(sum(ex1.^2,1)/MN); %EX2=sqrt(sum(ex2.^2,1)/MC); EX3=sqrt(sum(ex3.^2,1)/MN); % EY1=sqrt(sum(ey1.^2,1)/MC); % EY2=sqrt(sum(ey2.^2,1)/MC); EY3=sqrt(sum(ey3.^2,1)/MN); %{ mex1=mean(ex1); mey1=mean(ey1);%CV mex2=mean(ex2); mey2=mean(ey2);%CT %} mex3=mean(ex3); mey3=mean(ey3);%IMM mvx3=mean(vx3); mvy3=mean(vy3); Um1=mean(UM1); Um2=mean(UM2); t=1:5; figure(1) plot(xA(1,t),xA(3,t),'r',X_imm(1,t),X_imm(3,t),'k'); legend('真实值', 'IMM滤波',3);
评论
    相关推荐
    • 2019(核)粒子滤波目标跟踪算法综述_昝孟恩.rar
      2019年的核心期刊,关于粒子滤波的各种改进,重采样,交互多模型,自适应等方面
    • libiconv-1.1.tar.gz
      字符集转换程序
    • VisualC++.rar
      vc++数字图像处理 ,是一本很不错的介绍数字图像方面的书籍,这里有本书的全部源码
    • sharewareluncher.zip
      执行和去除共享软件日期限制的程序
    • opctkit.rar
      opc client 的开发工具
    • 一个XML留言本源代码(C#).rar
      用C#编写的XML源程序
    • VB6MultiThread.rar
      VB多线程源代码
    • ScrollBitmap_demo.zip
      Displaying a large bitmap file on a dialog box, in its original size, is quite difficult in the VC++ environment. However, it is possible to display a large bitmap to a predefined area of the dialog by using the StretchBlt( ) function.The major disadvantage of this is that the clarity of the image will be lost. Check out this article for displaying large bitmaps into the desired area of your dialog box in its original size with a scrolling technique used to show the entire bitmap. 滚动显示位图 在VC++环境下,在一个对话框中显示一个原始尺寸的大小的位图文件相当是困难的。然而,通过使用 StretchBlt()函数一个给定的区域显示一个大的位图是可能的。主要的缺点是图像将会失真。看了这篇通过卷动技术显示整个位图技术的文章,你将能够以它的原始尺寸在给定对话框的区域内显示一个大位图。 来源: http://www.codeguru.com/bitmap/ScrollBitmap.html