fangyeweisqx.m

MATLAB程序仅供大家参考。多多支持谢谢谢谢

function fangyeweisqx

close all 
clear all;
a=4.63e3;b=1.2765e4; 
c=sqrt(a^2+b^2); 
e=c/a;
p=b^2/a;
rp=a*(e-1);
ceita=acosd(-1/e);
u=3.253e5;%行星的引力常数
R=6.05e3;%行星的半径
vwuqiong=sqrt(u/a);
C='blue'; 
y=16000:-.1:13000; 
x=-sqrt(1+y.^2/b^2)*a+a*e;

T=1;   %定义的采样周期
k=1;
for i=1:length(y)
    if (x(i)<0&y(i)>0)
        jiaodu(i)=180+atand(y(i)/x(i));
    elseif x(i)<0&y(i)<0
        jiaodu(i)=-180+atand(y(i)/x(i));
    else
        jiaodu(i)=atand(y(i)/x(i));
    end
     if i==1
            t(i)=0;
            max=sqrt(a^3/u)*((e*sqrt(e^2-1)*sind(jiaodu(i))/(1+e*cosd(jiaodu(i))))-log((sqrt(e+1)+sqrt(e-1)*tand(jiaodu(i)/2))/(sqrt(e+1)-sqrt(e-1)*tand(jiaodu(i)/2))));
     end
    
  if i>1
    t(i)=max-(sqrt(a^3/u)*((e*sqrt(e^2-1)*sind(jiaodu(i))/(1+e*cosd(jiaodu(i))))-log((sqrt(e+1)+sqrt(e-1)*tand(jiaodu(i)/2))/(sqrt(e+1)-sqrt(e-1)*tand(jiaodu(i)/2)))));
    if k*T<t(i)&k*T>t(i-1)
       xx(k)=(x(i)+x(i-1))/2;
       yy(k)=(y(i)+y(i-1))/2;
       vx(k)=(x(i)-x(i-1))/(t(i)-t(i-1));
       vy(k)=(y(i)-y(i-1))/(t(i)-t(i-1));
       vexture(k)=sqrt(u*(2/sqrt(xx(k)^2+yy(k)^2)+1/a))-sqrt(vx(k)^2+vy(k)^2);
       
       tt(k)=(t(i)+t(i-1))/2;
        if (xx(k)<0&y(k)>0)
            j1(k)=180+atand(yy(k)/xx(k));     
         elseif x(i)<0&y(i)<0
            j1(k)=-180+atand(yy(k)/xx(k));
          else
            j1(k)=atand(yy(k)/x(k));
        end 
       
        j2(k)=2*asind(R/sqrt(xx(k)^2+yy(k)^2));
        
%     h11=-yy(k)/(xx(k)^2+yy(k)^2);
%     h12=xx(k)/(xx(k)^2+yy(k)^2);
%     h21=-2*R*xx(k)/(sqrt(xx(k)^2+yy(k)^2-R^2)*(xx(k)^2+yy(k)^2)); 
%     h22=-2*R*yy(k)/(sqrt(xx(k)^2+yy(k)^2-R^2)*(xx(k)^2+yy(k)^2));
%     H=[h11 h12 0 0 ;                  
%        h21 h22 0 0 ;];
%      jh1(k)=xx(k)*h11+yy(k)*h12
%      jh2(k)=xx(k)*h21+yy(k)*h22;
     
        k=k+1;
    end
  end
end
count=k;
nj1=(3)*randn(count-1,1);
nj2=(3)*randn(count-1,1);
zj1=j1+nj1';zj2=j2+nj2';

hold on; 
plot(x,y,'linewidth',2,'color',C);
plot([x(1)-10000 -x(1)+10000],[0 0],'linewidth',2,'color','black')    %x轴
fill([-x(1)+10000,-x(1)+10000+6000,-x(1)+10000],[2000,0,-2000],'k')%箭头 
plot([0 0],[-y(1) y(1)],'linewidth',2,'color','black')      %y轴
fill([-2000,0,2000],[-y(1)-6000,-y(1),-y(1)-6000],'k');%箭头 
plot([x(1) a*e],[(-x(1)+a*e)*tand(180-ceita) 0],'k--','linewidth',1) ;
plot([x(1) a*e],[-(-x(1)+a*e)*tand(180-ceita) 0],'k--','linewidth',1) ;



R=6.05e3;
ry=-R:1:R;
rx=sqrt(R^2-ry.^2);
plot(rx,ry,'linewidth',2,'color','y')
plot(-rx,ry,'linewidth',2,'color','y')
fill(rx,ry,'y')
fill(-rx,ry,'y')
plot([0 0],[-R R],'y','linewidth',2)
% text(0,0,'行星','fontsize',10,'fontname','宋体'); 
axis equal
figure(2)
plot(tt,xx,'linewidth',2)
grid on
xlabel('时间和x');
figure(3)
plot(tt,vx,'linewidth',2)
grid on
xlabel('时间和速度x');
figure(5)
plot(tt,vy,'linewidth',2,'color',C)
xlabel('时间和速度y');
figure(6)
plot(tt,j1,'o')
xlabel('时间和角1');
figure(7)
plot(tt,j2,'o')
xlabel('时间和角2');
% figure(8)
% plot(tt,jh1,'b')
% xlabel('时间和离散化后的角1');
% figure(9)
% plot(tt,jh2,'b')
% xlabel('时间和离散化后的角2');
figure(9)
plot(tt,vexture,'b')
xlabel('计算的速度与实际速度的差');




%***********************开始滤波过程***************************
A=[1 0 T 0;
   0 1 0 T;
   0 0 1 0;
   0 0 0 1;];
xc=[x(1) y(1) vx(1)+0 vy(1)+0]';
pc=[100 0 0 0;
    0 100 0 0;
    0 0 1 0;
    0 0 0 1;];
Q=diag([10 10 1 1]);
Q2=diag([3 3]);
% Q=diag([100 100 1 1]);
% R=diag([3e-3^2 3e-3^2]);
qtemp=rand(count,1);
rtemp=rand(count,1);
for k = 2:count-2
	% predict
	xp = A*xc;
	pp = A*pc*A'+ Q*qtemp(k);
%   pp = A*pc*A'+Q;
	% measurement prediction and Jacobian
	h11=-xp(2)/(xp(1)^2+xp(2)^2);
    h12=xp(1)/(xp(1)^2+xp(2)^2);
    h21=-2*R*xp(1)/(sqrt(xp(1)^2+xp(2)^2-R^2)*(xp(1)^2+xp(2)^2)); 
    h22=-2*R*xp(2)/(sqrt(xp(1)^2+xp(2)^2-R^2)*(xp(1)^2+xp(2)^2));
    H=[h11 h12 0 0 ;                  
       h21 h22 0 0 ;];
	% correct  
	K = pp*H'*inv(H*pp*H'+ Q2*rtemp(k)); % Kalman gain
    h=[atand(xp(2)/xp(1)) 2*asind(R/sqrt(xp(1)^2+xp(2)^2))]'
	z=[j1(k) j2(k)]';
    plotex(k-1)=xc(1)-x(k-1);
    plotey(k-1)=xc(2)-y(k-1);
    plotx(k-1)=xc(1);
    ploty(k-1)=xc(2);
    plotvx(k-1)=xc(3);
    plotvy(k-1)=xc(4);
    xc=xp+K*(z-h);
    z-h
	pc=(eye(4)-K*H)*pp;
   
end


figure(22)
i=1:length(plotex);
plot(i,plotex)
xlabel('x方向上的误差');
figure(33)
i=1:length(plotey);
plot(i,plotey)
xlabel('y方向上的误差');
figure(42)
i=1:length(plotx);
plot(i,plotvx)
xlabel('x速度的估计');
figure(55)
i=1:length(plotvy);
plot(i,plotvy)
xlabel('y速度的估计');

figure(66)
hold on 
% i=1:length(plotx);
plot(plotx,ploty,'r--')
plot(x,y,'b');
xlabel('xy的估计');

% figure(7)
% hold on;
% i=1:length(ploty);
% plot(i,ploty,'r-');
% 
% i=length(y);
% plot(i,y,'b*');
% xlabel('y的估计');