test_1_mine_5.zip_Mine!_测量误差

%三维问题进行编程 %恒速，v一定 clc clear %----------------对各个量进行初始化------------ T=0.1; %采样间隔 N=50;%采样次数 x=zeros(6,N);x(:,1)=[100;100;250;-50;0;50];%加噪声之后的距离的初始化 p(:,:,1)=zeros(6); p(:,:,1)=[ 10 0 0 0 0 0; 0 30 0 0 0 0; 0 0 50 0 0 0; 0 0 0 3 0 0; 0 0 0 0 50 0; 0 0 0 0 0 3000000000000000000000000];%初始方差的初始化 X=zeros(3,N);X(:,1)=[100;250;0];%真实距离的初始化 Q=[0.01 0 0;0 1 0;0 0 1]; R=[20 0 0;0 500 0;0 0 50];%两个白噪声的方差 A=[ 1 T 0 0 0 0; 0 1 0 0 0 0; 0 0 1 T 0 0; 0 0 0 1 0 0; 0 0 0 0 1 T; 0 0 0 0 0 1];%一步转移阵的初始化 L=[T/2 0 0;1 0 0;0 T/2 0;0 1 0;0 0 T/2;0 0 1];%系统噪声的驱动阵 H=[1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0];%量测阵 z=zeros(3,N);z(:,1)=[100;250;0];%量测值的初始化 W=zeros(3,N); W(1,:)=sqrtm(Q(1,1))*randn(1,N); W(2,:)=sqrtm(Q(2,2))*randn(1,N); W(3,:)=sqrtm(Q(3,3))*randn(1,N); V=zeros(3,N); V(1,:)=sqrtm(R(1,1))*randn(1,N); V(2,:)=sqrtm(R(2,2))*randn(1,N); V(3,:)=sqrtm(Q(3,3))*randn(1,N); %--------------计算真实值，量测值等----------- for t=2:N T1(t-1)=t*T; x(:,t)=A*x(:,t-1)+L*W(:,t-1);%计算添加噪声之后量测值的矩阵 X(1,t)=x(1,t); X(2,t)=x(3,t); X(3,t)=x(5,t); z(:,t)=H*x(:,t)+V(:,t);%计算量测距离的矩阵 end %--------------进行卡尔曼滤波------------------- P(:,:,1)=zeros(6);xx=zeros(6,N); for k=2:N P(:,:,k-1)=A*p(:,:,k-1)*A'+L*Q*L'; K(:,:,k)=P(:,:,k-1)*H'*inv(H*P(:,:,k-1)*H'+R); p(:,:,k)=(eye(6)-K(:,:,k)*H)*P(:,:,k-1); xx(:,k)=A*x(:,k-1); x(:,k)=xx(:,k)+K(:,:,k)*(z(:,k)-H*xx(:,k)); %以上五个是卡尔曼滤波的基本方程 end figure plot3(X(1,:),X(2,:),X(3,:),'k'); hold on plot3(z(1,:),z(2,:),z(3,:),'-.v'); hold on plot3(x(1,:),x(3,:),x(5,:),'r:*'); hold on title('飞行器所在的位置');xlabel('北方向');ylabel('东方向'),zlabel('垂直方向') grid on legend('飞行器实际位置','飞行器量测位置','飞行器滤波后位置',4) axis([0,600,-50,300,0,300]) figure plot3(X(1,:),X(2,:),X(3,:),'k'); hold on plot3(z(1,:),z(2,:),z(3,:),'-.v'); hold on plot3(x(1,:),x(3,:),x(5,:),'r:*'); hold on view(0,0) figure plot3(X(1,:),X(2,:),X(3,:),'k'); hold on plot3(z(1,:),z(2,:),z(3,:),'-.v'); hold on plot3(x(1,:),x(3,:),x(5,:),'r:*'); hold on view(0,90) figure plot3(X(1,:),X(2,:),X(3,:),'k'); hold on plot3(z(1,:),z(2,:),z(3,:),'-.v'); hold on plot3(x(1,:),x(3,:),x(5,:),'r:*'); hold on view(90,0)