shujuronghe-matlab.rar_matlab信息融合_信息滤波器_信息融合_信息融合 matlab_图像融合

A=[0 0 0 0 1/6370000 0 0 0 0 0 0 0 0 0 0; 1/6370000*sec(2.02)*tan(2.02) 0 -1/6370000^2*sec(2.02) sec(2.02)/6370000 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0; 0 0 0 0 2*(7.27e-5)*sin(2.02)+1/6370000*tan(2.02) -(2*7.27e-5*cos(2.02)+1/6370000) 0 0 0 0 0 0 1 0 0; -(2*7.27e-5*cos(2.02)+(sec(2.02))^2/6370000) 0 tan(2.02)/6370000^2 -(2*7.27e-5*sin(2.02)+1/6370000*tan(2.02)) 0 0 0 0 0 0 0 0 0 1 0; -2*7.27e-5*sin(2.02) 0 1/6370000^2 2*7.27e-5*cos(2.02)+1/6370000 0 0 0 0 0 0 0 0 0 0 1; 0 0 0 0 -1/6370000 0 0 7.27e-5*sin(2.02)+1/6370000*tan(2.02) -(7.27e-5*cos(2.02)+1/6370000) 1 0 0 0 0 0; -7.27e-5*sin(2.02) 0 -1/6370000^2 1/6370000 0 0 -(7.27e-5*sin(2.02)+1/6370000*tan(2.02)) 0 0 0 1 0 0 0 0; 7.27e-5*cos(2.02)+1/6370000*(sec(2.02))^2 0 -tan(2.02)/6370000^2 tan(2.02)/6370000 0 0 7.27e-5*cos(2.02)+1/6370000 0 0 0 0 1 0 0 0; 0 0 0 0 0 0 0 0 0 -1/300 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 -1/300 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1/300; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]; T=1;%滤波周期为1s F=eye(15,15)+A.*T; B=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0]; C=[1 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0]; D=0; g=9.7804; P=[(0.00254)^2 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 (0.00446)^2 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0.1^2 0 0 0 0 0 0 0 0 0 0 0 ; 0 0 0 0 0.1^2 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 (1)^2 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 (1)^2 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 (1)^2 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 (0.1)^2 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 (0.1)^2 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 (0.1)^2 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 (1e-4*g)^2 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 (1e-4*g)^2 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 (1e-4*g)^2]; Q=[(0.01)^2 0 0 0 0 0; 0 (0.01)^2 0 0 0 0; 0 0 (0.01)^2 0 0 0; 0 0 0 (0.05)^2 0 0; 0 0 0 0 (0.05)^2 0; 0 0 0 0 0 (0.05)^2]; R=[(0.0254)^2 0 0 0 0 0; 0 (0.0446)^2 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0.47^2 0 0; 0 0 0 0 0.47^2 0; 0 0 0 0 0 0]; t = [0:1000]'; u = 0*sin(t/5); % Generate process noise and sensor noise vectors randn('seed',0) w = sqrt(Q)*(randn(length(t),6))'; %subplot(311), plot(t,w(1,:));subplot(312), plot(t,w(2,:));subplot(313), plot(t,w(3,:));pause; v = sqrt(R)*(randn(length(t),6))'; % Generate the noisy plant response %sys = ss(A,B,C,D,-1); %[y,T,x0] = lsim(sys,w'); % w = process noise x0=zeros(15,length(t)); x0(:,1)=[100 100 0 0.5 0.5 0 0.08 0.08 0.16 0.1 0.1 0.1 0.0001*g 0.0001*g 0.0001*g]'; x2=zeros(15,1); yv(:,1)=C*x2(:,1)+v(:,1); for i=2:length(t) %x00(:,i)=A*x0(:,i-1); x2(:,i)=F*x2(:,i-1)+B*w(:,i); % yv(:,i)=C*x2(:,i)+v(:,i); end %subplot(331), plot(t,x0(1,:)); %subplot(332), plot(t,x0(2,:)); %subplot(333), plot(t,x0(3,:)); %plot(t,x0(1,:)); %plot(t,x0(2,:)); %plot(t,x0(4,:)); % Now implement the filter recursions in a FOR loop: % Initial error covariance %x=zeros(15,1); % Initial condition on the state x=x0(:,1); %ye = zeros(2,length(t)); for i=1:length(t) P = F*P*F'+B*Q*B'; % P[n+1|n] K = P*C'*inv(C*P*C'+R); x = F*x; % x[n+1|n] x = x + K*(yv(:,i)-C*x); % x[n|n] P = (eye(15)-K*C)*P; % P[n|n] x1(:,i)=x; %ye(:,i) = C*x; end %subplot(211), plot(t,y,'b',t,ye,'r--'), %xlabel('No. of samples'), ylabel('Output') %subplot(334), plot(t,x1(1,:),'r--',t,x0(1,:),'-'); %subplot(335), plot(t,x1(2,:),'r--',t,x0(2,:),'-.'); %subplot(336), plot(t,x1(3,:),'r--',t,x0(3,:),'-.'); %plot(t,x0(1,:),'-',t,x1(1,:),'r-'); %axis([0,50,99.8,100.1]); %绘制经度误差曲线 %plot(t,x1(1,:),'-',t,yv(1,:),'-.'); %plot(t,yv(1,:),'-'); %axis([0,1000,-0.5,0.5]); %xlabel('采样数（个）'), ylabel('经度滤波前误差(米)'); %plot(t,abs(yv(1,:)-x1(1,:)),'-'); %axis([0,1000,-0.5,0.5]); %xlabel('采样数（个）'), ylabel('经度误差校正幅度(米)'); %legend('after filter','before filter',1); %subplot(311),plot(t,yv(1,:),'-.'); %axis([0,1000,-0.3,0.3]); %xlabel('采样数（个）'), ylabel('经度滤波前误差(米)'); %subplot(312),plot(t,x1(1,:),'-'); %axis([0,1000,-0.3,0.3]); %xlabel('采样数（个）'), ylabel('经度滤波后误差(米)'); %subplot(313),plot(t,abs(yv(1,:)-x1(1,:)),'-'); %axis([0,1000,-0.3,0.3]); %xlabel('采样数（个）'), ylabel('经度误差校正幅度(米)'); %绘制纬度误差曲线 %plot(t,x1(2,:),'-',t,yv(2,:),'-.'); %axis([0,1000,98,102]); %xlabel('No. of Samples'), ylabel('Error of Latitude'); %subplot(311),plot(yv(2,:),'-.'); %axis([0,1000,-0.3,0.3]); %xlabel('采样数（个）'), ylabel('纬度滤波前误差(米)'); %legend('after filter','before filter',1); %subplot(312),plot(t,x1(2,:),'-'); %axis([0,1000,-0.3,0.3]); %xlabel('采样数（个）'), ylabel('纬度滤波后误差(米)'); %subplot(313),plot(t,abs(yv(2,:)-x1(2,:)),'-'); %axis([0,1000,-0.3,0.3]); %xlabel('采样数（个）'), ylabel('纬度误差校正幅度(米)'); %绘制东向速度误差曲线 %plot(t,x1(4,:),'-'); %plot(t,abs(yv(4,:)-x1(4,:)),'-'); %xlabel('采样数（个）'), ylabel('东向速度误差校正幅度(米/秒)'); %legend('after filter','before filter',1); %subplot(311),plot(t,yv(4,:),'-.'); %xlabel('采样数（个）'), ylabel('东向速度滤波前误差(米/秒)'); %legend('after filter','before filter',1); %subplot(312),plot(t,x1(4,:),'-'); %xlabel('采样数（个）'), ylabel('东向速度滤波后误差(米/秒)'); %subplot(313),plot(t,abs(yv(4,:)-x1(4,:)),'-'); %xlabel('采样数（个）'), ylabel('东向速度误差校正幅度(米/秒)'); %绘制北向速度误差曲线 subplot(311),plot(t,yv(5,:),'-.'); xlabel('采样数（个）'), ylabel('北向速度滤波前误差(米/秒)'); %legend('after filter','before filter',1); subplot(312),plot(t,x1(5,:),'-'); xlabel('采样数（个）'), ylabel('北向速度滤波后误差(米/秒)'); subplot(313),plot(t,abs(yv(5,:)-x1(5,:)),'-'); xlabel('采样数（个）'), ylabel('北向速度误差校正幅度(米/秒)'); %绘制东向姿态角误差曲线 %subplot(121),plot(t,x1(7,:),'-',t,yv(7,:),'-.'); %xlabel('采样数（个）'), ylabel('东向姿态角滤波前/后误差(度)'); %legend('after filter','before filter',1); %subplot(122),plot(t,abs(yv(7,:)-x1(7,:)),'-'); %xlabel('采样数（个）'), ylabel('东向姿态角误差校正幅度(度)'); aa=abs(x1(1,:)-x0(1,:)); bb=abs(x1(2,:)-x0(2,:)); cc=abs(x1(3,:)-x0(3,:)); %subplot(337), plot(t,aa,'r-'); %subplot(338), plot(t,bb,'r-'); %subplot(339), plot(t,cc,'r-'); %pause;subplot(111);plot(t,x1(2,:),'r--',t,x0(2,:),'-.');pause;subplot(111);plot(t,x1(2,:),'r--',t,x0(2,:),'-.');pause;subplot(111);plot(t,x1(3,:),'r--',t,x0(3,:),'-.'); %pause; %subplot(111) %plot(t,aa,'r-');