31128479Multi-sensor-data-fusion_传感器融合_传感器_传感器融合_datafusion_多传感器

%参考文献 %多传感器数据融合及其应用（杨万海著） %P45-46页 % D R的确定方法见P41页 2-1-102到104 clear all; clc; T=5; % 采样周期 N=40; % 采样点数 x0=20; %目标在x方向的初始位置为20公里 y0=20; %目标在y方向的初始位置为20公里 v0x= -0.2; %目标在x方向的速度 v0y=0; %目标在y方向的速度 deta_r=250; %观测距离误差的标准方差 deta_theta=0.0005; %观测方位角误差的标准方差 %装载观测数据 load trace.mat; for k=1:N xx1(k)=xx(k,1)/1000 xx2(k)=xx(k,2)/1000 end Z_T=[xx1;xx2]; temp=zeros(2,40); F=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1]; Q=[0.001 0 0 0;0 0.001 0 0;0 0 0.001 0;0 0 0 0.001]; I=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; %计算观测噪声x,y的协方差矩阵R R=[0.25 0; 0 0.0001]; %注意每次循环都要给X0和P0赋初值 X0=[x0,y0,v0x,v0y]'; %状态向量初值 P0=[1.0 0 0 0;0 1.0 0 0;0 0 0.01 0;0 0 0 0.01]; %估计误差协方差阵初值 for t=1:N-1 Xk_predict=F*X0; %预测时，使用直角坐标系 %坐标转换，转化成极坐标 xm=Xk_predict(1); ym=Xk_predict(2); Z_predict=[sqrt(xm^2+ym^2),atan(ym/xm)]'; %[THETA,PHI,R] = cart2sph(xm,ym,0); %计算测量矩阵 r2=sqrt(xm^2+ym^2); h1=[xm/r2, -ym/r2^2]'; h2=[ym/r2, xm/r2^2]'; O2=[0,0]'; H=[h1,h2,O2,O2]; %预测协方差阵 Pk_predict=F*P0*F'+Q; S=H*Pk_predict*H'+R; %信息协方差阵 K=Pk_predict*H'*inv(S); %增益矩阵 temp(:,t)=Z_T(:,t)-Z_predict; Xk(:,t)=Xk_predict+K*(Z_T(:,t)-Z_predict); %估计矩阵(最后的输出值) Pk=(eye(4)-K*H)*Pk_predict; %估计误差协方差阵 P0=Pk; %估计误差协方差阵 X0=Xk(:,t); %估计矩阵更新 end for k=1:N-1 x_filter(k)=Xk(1,k); y_filter(k)=Xk(2,k); vx_filter(k)=Xk(3,k); vy_filter(k)=Xk(4,k); end %直角坐标系到极坐标系的转换 [THETA,PHI,Ro] = cart2sph(x_filter,y_filter,0); [traceX,traceY,traceZ] = sph2cart(xx2,0,xx1); figure(1) plot(traceX);zoom on;grid on; hold on; plot(x_filter,'r:');zoom on;grid on; xlabel('点数') ylabel('距离') title('经过卡尔曼滤波以后的x方向上的距离') % figure(2) % plot(guance_y);zoom on;grid on; % hold on; % plot(y_filter2,'r:');zoom on;grid on; % xlabel('点数') % ylabel('距离') % title('经过卡尔曼滤波以后的y方向上的距离') % % figure(3) % plot(Ro);zoom on;grid on; % xlabel('点数') % ylabel('距离') % title('经过卡尔曼滤波以后的极坐标下的观测') % % figure(4) % plot(vx_filter1);zoom on;grid on; % hold on; % plot(vx_filter2,'r:');zoom on;grid on; % xlabel('点数') % ylabel('速度') % title('x方向上的速度滤波值') % % figure(5) % plot(vy_filter1);zoom on;grid on; % hold on; % plot(vy_filter2,'r:');zoom on;grid on; % xlabel('点数') % ylabel('速度') % title('y方向上的速度滤波值')