quxianxyj.m

曲线的卡尔曼滤波程序仅供参考。谢谢大家的支持。要是好的话多支持我啊

function quxianxyj
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=60000:-.1:57000; 
x=-sqrt(1+y.^2/b^2)*a+a*e;

T=2;   %定义的采样周期
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));
       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));
        k=k+1;
    end
  end
end
count=k;
nj1=(3e-3)^2*randn(count-1,1);
nj2=(3e-3)^2*randn(count-1,1);
zj1=j1+nj1';zj2=j2+nj2';

nx=10*randn(count-1,1);
ny=10*randn(count-1,1);
zx=xx+nx';zy=yy+ny';

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');
%******************************以下部分进行Kalman滤波******************
xc=[xx(1)+200 yy(1)+100 vx(1)-2 vy(1)-2]';
pc=[100 0 0 0;
    0 100 0 0;
    0 0 1 0;
    0 0 0 1;];
s=1.1;
%pc=pc.*s^(count-2);
Q=diag([10 10 1 1]);
Q2=diag([3e-3 10]);
% Q2=diag([1 1]);
qtemp=randn(count,1);
rtemp=randn(count,1);

for k = 2:count-2
	% predict
     f=[xc(3) xc(4) -u*xc(1)/(sqrt(xc(1)^2+xc(2)^2))^3 -u*xc(2)/(sqrt(xc(1)^2+xc(2)^2))^3 ]';
     xp=xc+f*T;
     k1=u*(2*xp(1)^2-xp(2)^2)*T/(sqrt(xp(1)^2+xp(2)^2))^5;
     k2=3*u*xp(1)*xp(2)*T/(sqrt(xp(1)^2+xp(2)^2))^5;
     k3=3*u*xp(1)*xp(2)*T/(sqrt(xp(1)^2+xp(2)^2))^5;
     k4=u*(2*xp(2)^2-xp(1)^2)*T/(sqrt(xp(1)^2+xp(2)^2))^5;
         
     A=[1 0  T 0 ;%状态转移矩阵
        0 1  0 T ;
        k1 k2  1 0 ;
        k3 k4  0 1 ;];
    
	pp = A*pc*A'+ Q*qtemp(k)       %*s^(count-2-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 ;                  
         0 1 0 0 ;];
	% correct  
    K = pp*H'*inv(H*pp*H'+ Q2*rtemp(k));% 	*s^(count-2-k)); % Kalman gain
    h=[atand(xp(2)/xp(1)) xp(2)]'
	z=[zj1(k) zy(k)]';
    plotex(k-1)=xc(1)-xx(k-1);
    plotey(k-1)=xc(2)-yy(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);
	pc=(eye(4)-K*H)*pp;%*(eye(4)-K*H)'+K*Q2*K';
   
end
figure(20)
i=1:length(plotex);
plot(i,plotex)
xlabel('x误差');
figure(30)
i=1:length(plotey);
plot(i,plotey)
xlabel('y误差');
figure(40)
i=1:length(plotx);
plot(i,plotx)
xlabel('x估计');
figure(50)
i=1:length(ploty);
plot(i,ploty)
xlabel('y估计');
figure(60)
plot(xx,yy,'r')
hold on 
plot(plotx,ploty,'b--')