自适应卡尔曼滤波实例，MATLAB

D0=eye(4); dt0=5; m0=2; X11=[26422.6886;47431.1625;0;0]; L1=[26422.6886,26422.6920,26422.6872,26422.6866,26422.6818;47431.1625,47431.1635,47431.1600,47431.1602,47431.1590]; [ZZ0,Z0]=kf(D0,X11,dt0,m0,L1); [ZZ1,Z1]=zkf(D0,X11,dt0,m0,L1); % plot(1:5,L1(1,:),'-.dg',1:5,Z0(1,:),'--sr',1:5,Z1(1,:),'-^k'); % axis([0.5,5.5,26422.6760,26422.6940]); % set(gca,'XTick',0.5:1: 5.5, 'YTick',26422.6760:0.0020:26422.6940); % set(gca, 'YGrid','on'); % set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); % title('B01X'); % legend('实测','标准','方差补偿') c0=abs(L1(1,:)-Z0(1,:))*1000; c1=abs(L1(1,:)-Z1(1,:))*1000; c00=abs(L1(2,:)-Z0(2,:))*1000; c11=abs(L1(2,:)-Z1(2,:))*1000; figure subplot(1,2,1) plot(1:5,c0,'--sr',1:5,c1,'-^k'); axis([0.5,5.5,0,0.1]); set(gca,'XTick',0:1: 6, 'YTick',0:0.02:0.1); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('X坐标偏离值'); legend('卡尔曼','自适应卡尔曼') subplot(1,2,2) plot(1:5,c00,'--sr',1:5,c11,'-^k'); axis([0.5,5.5,0,0.06]); set(gca,'XTick',0:1: 6, 'YTick',0:0.015:0.06); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('Y坐标偏离值'); legend('卡尔曼','自适应卡尔曼') X12=[26435.5028;47457.9285;0;0]; L2=[26435.5028,26435.5067,26435.4994,26435.4976,26435.4952;47457.9285,47457.9292,47457.9271,47457.9277,47457.9274]; [ZZ2,Z2]=kf(D0,X12,dt0,m0,L2); [ZZ3,Z3]=zkf(D0,X12,dt0,m0,L2); % figure % plot(1:5,L2(1,:),'-.dg',1:5,Z2(1,:),'--sr',1:5,Z3(1,:),'-^k'); % axis([0.5,5.5,26435.4880,26435.5080]); % set(gca,'XTick',0.5:1: 5.5, 'YTick',26435.4880:0.0020:26435.5080); % set(gca, 'YGrid','on'); % set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); % title('B01X'); % legend('实测','标准','方差补偿') c2=abs(L2(1,:)-Z2(1,:))*1000; c3=abs(L2(1,:)-Z3(1,:))*1000; c22=abs(L2(2,:)-Z2(2,:))*1000; c33=abs(L2(2,:)-Z3(2,:))*1000; figure subplot(1,2,1) plot(1:5,c2,'--sr',1:5,c3,'-^k'); axis([0.5,5.5,0,0.1]); set(gca,'XTick',0:1: 6.5, 'YTick',0:0.02:0.1); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('X坐标偏离值'); legend('卡尔曼','自适应卡尔曼') subplot(1,2,2) plot(1:5,c22,'--sr',1:5,c33,'-^k'); axis([0.5,5.5,0,0.04]); set(gca,'XTick',0:1: 6, 'YTick',0:0.01:0.04); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('Y坐标偏离值'); legend('卡尔曼','自适应卡尔曼') X13=[26442.8465;47497.0982;0;0]; L3=[26442.8465,26442.8483,26442.8422,26442.8467,26442.8424;47497.0982,47497.0992,47497.1000,47497.0988,47497.0982]; [ZZ4,Z4]=kf(D0,X13,dt0,m0,L3); [ZZ5,Z5]=zkf(D0,X13,dt0,m0,L3); c4=abs(L3(1,:)-Z4(1,:))*1000; c5=abs(L3(1,:)-Z5(1,:))*1000; c44=abs(L3(2,:)-Z4(2,:))*1000; c55=abs(L3(2,:)-Z5(2,:))*1000; figure subplot(1,2,1) plot(1:5,c4,'--sr',1:5,c5,'-^k'); axis([0.5,5.5,0,0.14]); set(gca,'XTick',0:1: 6, 'YTick',0:0.02:0.14); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('X坐标偏离值'); legend('卡尔曼','自适应卡尔曼') subplot(1,2,2) plot(1:5,c44,'--sr',1:5,c55,'-^k'); axis([0.5,5.5,0,0.02]); set(gca,'XTick',0:1: 6, 'YTick',0:0.005:0.02); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('Y坐标偏离值'); legend('卡尔曼','自适应卡尔曼') X14=[26417.7600;47562.0978;0;0]; L4=[26417.7600,26417.7614,26417.7612,26417.7562,26417.7599;47562.0978,47562.1017,47562.0979,47562.1013,47562.0970]; [ZZ6,Z6]=kf(D0,X14,dt0,m0,L4); [ZZ7,Z7]=zkf(D0,X14,dt0,m0,L4); c6=abs(L4(1,:)-Z6(1,:))*1000; c7=abs(L4(1,:)-Z7(1,:))*1000; c66=abs(L4(2,:)-Z6(2,:))*1000; c77=abs(L4(2,:)-Z7(2,:))*1000; figure subplot(1,2,1) plot(1:5,c6,'--sr',1:5,c7,'-^k'); axis([0.5,5.5,0,0.08]); set(gca,'XTick',0:1: 6, 'YTick',0:0.02:0.08); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('X坐标偏离值'); legend('卡尔曼','自适应卡尔曼') subplot(1,2,2) plot(1:5,c66,'--sr',1:5,c77,'-^k'); axis([0.5,5.5,0,0.12]); set(gca,'XTick',0:1: 6, 'YTick',0:0.02:0.12); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('Y坐标偏离值'); legend('卡尔曼','自适应卡尔曼') X15=[26318.9005;47700.8969;0;0]; L5=[26318.9005,26318.9020,26318.8993,26318.8989,26318.8986;47700.8969,47700.8986,47700.8948,47700.8950,47700.8970]; [ZZ8,Z8]=kf(D0,X15,dt0,m0,L5); [ZZ9,Z9]=zkf(D0,X15,dt0,m0,L5); c8=abs(L5(1,:)-Z8(1,:))*1000; c9=abs(L5(1,:)-Z9(1,:))*1000; c88=abs(L5(2,:)-Z8(2,:))*1000; c99=abs(L5(2,:)-Z9(2,:))*1000; figure subplot(1,2,1) plot(1:5,c8,'--sr',1:5,c9,'-^k'); axis([0.5,5.5,0,0.06]); set(gca,'XTick',0:1: 6, 'YTick',0:0.015:0.06); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('X坐标偏离值'); legend('卡尔曼','自适应卡尔曼') subplot(1,2,2) plot(1:5,c88,'--sr',1:5,c99,'-^k'); axis([0.5,5.5,0,0.06]); set(gca,'XTick',0:1: 6, 'YTick',0:0.015:0.06); set(gca, 'YGrid','on'); set(gca,'yticklabel',sprintf('%03.4f|',get(gca,'ytick'))); title('Y坐标偏离值'); legend('卡尔曼','自适应卡尔曼') yc=[ZZ1(1),ZZ3(1),ZZ5(1),ZZ7(1),ZZ9(1);ZZ1(2),ZZ3(2),ZZ5(2),ZZ7(2),ZZ9(2)];