singerCTCA模型卡尔曼滤波实现

clear;clc; %%%%%%%%%%%%%%雷达地心地固坐标计算%%%%%%%%%%%%%%%%%% [x01,y01,z01]=change(122.1,40.5,0);[x02,y02,z02]=change(122.4,41.5,0);[x03,y03,z03]=change(122.7,40.9,0); load('Data1.mat'); load('Data4.mat'); % %%%%%%%%%%%%1号雷达球-直角坐标变换%%%%%%%%% logic1=(Data1(:,5)==1); x1=Data1(logic1,1).*cos(pi/2-Data1(logic1,2)/180*pi).*cos(Data1(logic1,3)/180*pi); y1=Data1(logic1,1).*sin(pi/2-Data1(logic1,2)/180*pi).*cos(Data1(logic1,3)/180*pi); z1=Data1(logic1,1).*sin(Data1(logic1,3)/180*pi); % %%%%%%%%%%%%多项式拟合%%%%%%%%%%%%%%% tt1=Data1(logic1,4)-Data1(1,4); p=polyfit(tt1,y1,4); YY1=p(1)*tt1.^4+p(2)*tt1.^3+p(3)*tt1.^2+p(4)*tt1+p(5); p1=polyfit(tt1,x1,4); XX1=p1(1)*tt1.^4+p1(2)*tt1.^3+p1(3)*tt1.^2+p1(4)*tt1+p1(5); p2=polyfit(tt1,z1,4); ZZ1=p2(1)*tt1.^4+p2(2)*tt1.^3+p2(3)*tt1.^2+p2(4)*tt1+p2(5); %%%%%%%%%%%%2号雷达球-直角坐标变换%%%%%%%%% logic2=(Data1(:,5)==2); x2=Data1(logic2,1).*cos(Data1(logic2,2)/180*pi).*cos(Data1(logic2,3)/180*pi); y2=Data1(logic2,1).*sin(Data1(logic2,2)/180*pi).*cos(Data1(logic2,3)/180*pi); z2=Data1(logic2,1).*sin(Data1(logic2,3)/180*pi); % %%%%%%%%%%%%多项式拟合%%%%%%%%%%%%%%% tt2=Data1(logic2,4)-Data1(1,4); p=polyfit(tt2,y2,4); YY2=p(1)*tt2.^4+p(2)*tt2.^3+p(3)*tt2.^2+p(4)*tt2+p(5); p1=polyfit(tt2,x2,4); XX2=p1(1)*tt2.^4+p1(2)*tt2.^3+p1(3)*tt2.^2+p1(4)*tt2+p1(5); p2=polyfit(tt2,z2,4); ZZ2=p2(1)*tt2.^4+p2(2)*tt2.^3+p2(3)*tt2.^2+p2(4)*tt2+p2(5); % % %%%%%%%%%%%%3号雷达球-直角坐标变换%%%%%%%%% % logic3=(Data1(:,5)==3); x3=Data1(logic3,1).*cos(Data1(logic3,2)/180*pi).*cos(Data1(logic3,3)/180*pi); y3=Data1(logic3,1).*sin(Data1(logic3,2)/180*pi).*cos(Data1(logic3,3)/180*pi); z3=Data1(logic3,1).*sin(Data1(logic3,3)/180*pi); % figure(1) % plot3([x01,x02,x03],[y01,y02,y03],[z01,z02,z03],'o','MarkerFaceColor','r','MarkerEdgeColor','k') % str=['ld1';'ld2';'ld3']; % text([x01,x02,x03]+1e3,[y01,y02,y03]+1e3,[z01,z02,z03]+1e3,cellstr(str)); % xlabel('x');ylabel('y');zlabel('z'); % hold on % grid on % %%%%%%%%%%%%多项式拟合%%%%%%%%%%%%%%% tt3=Data1(logic3,4)-Data1(1,4); p=polyfit(tt3,y3,4); YY3=p(1)*tt3.^4+p(2)*tt3.^3+p(3)*tt3.^2+p(4)*tt3+p(5); p1=polyfit(tt3,x3,4); XX3=p1(1)*tt3.^4+p1(2)*tt3.^3+p1(3)*tt3.^2+p1(4)*tt3+p1(5); p2=polyfit(tt3,z3,4); ZZ3=p2(1)*tt3.^4+p2(2)*tt3.^3+p2(3)*tt3.^2+p2(4)*tt3+p2(5); tt=Data1(logic3,4)-Data1(1,4); TT=linspace(min(tt),max(tt),max(tt)-min(tt)+1); % % %%%%%%%%%%%%%%雷达站心坐标-地心地固坐标转换%%%%%%%%%%%%%%%%%% [xx1,yy1,zz1]=xyz2XYZ(x1,y1,z1,122.1,40.5,x01,y01,z01); [xx2,yy2,zz2]=xyz2XYZ(x2,y2,z2,122.4,41.5,x02,y02,z02); [xx3,yy3,zz3]=xyz2XYZ(x3,y3,z3,122.7,40.9,x03,y03,z03); % plot3(xx1,yy1,zz1); % plot3(xx2,yy2,zz2); % plot3(xx3,yy3,zz3); V=[xx1(1)-x01,yy1(1)-y01,zz1(1)-z01]; Vv=V/sqrt(sum(V.^2)) V1=[xx3(1)-x03,yy3(1)-y03,zz3(1)-z03]; Vv1=V1/sqrt(sum(V1.^2)) hold off % %%%%%%%%%%%%平移%%%%%%%%%%%%%% % zb2x=(xx1(size(xx1,2))-xx2(1)); zb2y=(yy1(size(yy1,2))-yy2(1)); zb2z=(zz1(size(zz1,2))-zz2(1)); xx2=xx2+zb2x*ones(1,size(xx2,2)); yy2=yy2+zb2y*ones(1,size(xx2,2)); zz2=zz2+zb2z*ones(1,size(xx2,2)); zb3x=(xx1(size(xx1,2))-xx3(1)); zb3y=(yy1(size(xx1,2))-yy3(1)); zb3z=(zz1(size(xx1,2))-zz3(1)); xx3=xx3+zb3x*ones(1,size(xx3,2)); yy3=yy3+zb3y*ones(1,size(xx3,2)); zz3=zz3+zb3z*ones(1,size(xx3,2)); figure(2) plot3(xx1,yy1,zz1) hold on plot3(xx2,yy2,zz2) plot3(xx3,yy3,zz3) xlabel('x');ylabel('y');zlabel('z'); plot3([x01,x02,x03],[y01,y02,y03],[z01,z02,z03],'o') hold off grid on % figure(3) [L1,B1]=pab2LB(Data1(logic3,1),Data1(logic3,2),Data1(logic3,3),122.7/180*pi,40.9/180*pi); [L2,B2]=pab2LB(Data1(logic3,1),Data1(logic3,2),Data1(logic3,3),122.7/180*pi,40.9/180*pi); %%%%%%%%%%%%%%%%%%%%%%singer model%%%%%%%%%%%%%%%%%%%%%%%%%%% T=1; Zk=[X ;Y; Z]; a=20000; XXa1=(T/a-1+exp(-T/a))*a^2; XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=20000; YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); a=20; ZZa1=(T/a-1+exp(-T/a))*a^2; ZZa2=(1-exp(-T/a))*a; ZZa3=exp(-T/a); Hk=[1 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 1 0 0]; Fk=[1 T XXa1 0 0 0 0 0 0; 0 1 XXa2 0 0 0 0 0 0; 0 0 XXa3 0 0 0 0 0 0; 0 0 0 1 T YYa1 0 0 0 ; 0 0 0 0 1 YYa2 0 0 0; 0 0 0 0 0 YYa3 0 0 0; 0 0 0 0 0 0 1 T ZZa1; 0 0 0 0 0 0 0 1 ZZa2; 0 0 0 0 0 0 0 0 ZZa3]; x=Zk(1,1);y=Zk(2,1);z=Zk(3,1);l=size(Zk,2)+1; xx=0;xxx=0;yy=0;yyy=0;zz=0;zzz=0; % diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); % yyy=diffy(1); % diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); % xxx=diffx(1); % diffz=diff(Zk(3,:),1);zz=diffz(1);diffz=diff(Zk(3,:),2); % zzz=diffz(1); P0=[4e6 0 0 0 0 0 0 0 0; 0 6e7 0 0 0 0 0 0 0; 0 0 4e5 0 0 0 0 0 0; 0 0 0 6e7 0 0 0 0 0; 0 0 0 0 4e9 0 0 0 0; 0 0 0 0 0 3e5 0 0 0; 0 0 0 0 0 0 6e3 0 0; 0 0 0 0 0 0 0 3e4 0; 0 0 0 0 0 0 0 0 8e3 ]; X0=[x xx xxx y yy yyy z zz zzz]';R=eye(3)*sqrt(40/3); Xkk1=zeros(9,l); Pkk=P0; Xkk=zeros(9,l);Xkk(:,1)=X0; for i=1:l-1 Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk'; Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R); Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(1) plot3(Zk(1,:),Zk(2,:),Zk(3,:),'b:'); hold on % plot3(Xkk(1,:),Xkk(4,:),Xkk(7,:),'r.-'); plot3(XX,YY,ZZ+3e2,'r.-') xlabel('x');ylabel('y');zlabel('z'); hold off grid on % % % % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%二维singer model%%%%%%%%%%%%%%%%%%%%%%%% % % % T=0.5; Zk=[XX1 YY1]'; a=2e1; XXa1=(T/a-1+exp(-T/a))*a^2; XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=9e6; YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); Hk=[1 0 0 0 0 0 ; 0 0 0 1 0 0 ;]; Fk=[1 T XXa1 0 0 0 ; 0 1 XXa2 0 0 0 ; 0 0 XXa3 0 0 0 ; 0 0 0 1 T YYa1 ; 0 0 0 0 1 YYa2 ; 0 0 0 0 0 YYa3 ;]; x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1; diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); yyy=diffy(1); diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1); P0=[50 1 2 3 4 5; 2 60 2 3 4 5; 1 2 70 3 2 3; 1 2 3 76 2 3; 1 2 1 3 76 6; 2 1 3 2 4 89]; X0=[x xx xxx y yy yyy]'; R=eye(2,2)*sqrt(40/2); Xkk1=zeros(6,l);Xkk=zeros(6,l);Xkk(:,1)=X0; Pkk=P0; % [45 0 0 0 0 0; % 0 54 0 0 0 0; % 0 0 65 0 0 0; % 0 0 0 76 0 0; % 0 0 0 0 87 0; % 0 0 0 0 0 98;]; for i=1:l-1 Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk'; Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R); Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(5) plot(Zk(1,:),Zk(2,:),'b:'); hold on plot(Xkk(1,:),Xkk(4,:),'r.-'); xlabel('x');ylabel('y'); hold off grid on % % % % % % %%%%%%%%%%%%%%%%%%%%%%%%ct model%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % % % T=1; % Zk=[x3 y3]'; % omega=2*pi/130; % Hk=[1 0 0 0 ; % 0 0 1 0;]; % Fk=[1 sin(omega)/omega 0 -(1-cos(omega))/omega ; % 0 cos(omega) 0 -sin(omega) ; % 0 (1-cos(omega))/omega 1 sin(omega)/omega ; % 0 sin(omega) 0 cos(omega) ;]; % x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1; % diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); % diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); % X0=[x xx y yy ]';Xkk=zeros(4,l);Xkk(:,1)=X0; % P0=[40 12 3 1; % 12 50 3 4; % 2 21 50 4; % 2 5 1 50]; % R=eye(2,2)*sqrt(40/2); % Xkk1=zeros(4,l);l=size(Zk,2); % Pkk=P0; % for i=1:l-1 % Xkk1(:,i)=Fk*Xkk(:,i); % Pkk1=Fk*Pkk*Fk'; % Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R); % Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); % Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; % % end % figure(4) % plot(Zk(1,:),Zk(2,:),'b:'); % hold on % plot(Xkk(1,1:l-1),Xkk(3,1:l-1),'r.'); % xlabel('x');ylabel('y'); % hold off % grid on %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%当前统计模型%%%%%%%%%%%%%