clear all;clc;Bushu=50;
   Q=0.5^2   
   R=13701;
   r=2.953*10^3;
   x=1;
   ata=500;UE=3.986*10^14;J2=1.083*10^(-3);RE=6.378*10^6;A0=1.5*J2*RE^2;
    randn('seed',1);w=sqrt(Q)*randn(1,Bushu);
    randn('seed',2);v=sqrt(R)*randn(1,Bushu);
   x1=1000,x2=1000,x3=1000,
   r=sqrt(x1^2+x2^2+x3^2)
a1x1=-UE./r^3+3*UE*x1^2/r^5-UE*A0/r^5+5*UE*A0*(x1^2+x3^2)/r^7-35*UE*A0*x1^2*x3^2/r^9
a1x2=3*UE*x1*x2/r^5-5*UE*A0*x1*x2/r^7-35*UE*A0*x1*x2*x3^2/r^9
a1x3=3*UE*x1*x3/r^5+5*UE*A0*x1*x3/r^7-35*UE*A0*x1*x3^3/r^9
a2x1=3*UE*x1*x2/r^5-5*UE*A0*x1*x2/r^7-35*UE*A0*x1*x2*x3^2/r^9
a2x2=-UE./r^3+(3*UE*x2^2-UE*A0)/r^5+5*UE*A0*(x2^2+x3^2)/r^7-35*UE*A0*x2^2*x3^2/r^9
a2x3=3*UE*x2*x3/r^5+5*UE*A0*x2*x3/r^7-35*UE*A0*x2*x3^3/r^9
a3x1=3*UE*x1*x3/r^5+15*UE*A0*x1*x3/r^7-35*UE*A0*x1*x3^3/r^9
a3x2=3*UE*x2*x3/r^5+15*UE*A0*x2*x3/r^7-35*UE*A0*x2*x3^3/r^9
a3x3=-UE./r^3+3*UE*x3^2/r^5-15*UE*A0*x3/r^5+10*UE*A0*x3/r^7-35*UE*A0*x3^3/r^9
fai=[1 0 0 ata 0 0;
     0 1 0 0  ata 0;
     0 0 1 0 0 ata;
     1+ata*a1x1     0+ata*a1x2  0+ata*a1x3  1  0  0;
     0+ata*a2x1     1+ata*a2x2  0+ata*a2x3  0  1  0;
     0+ata*a3x1     0+ata*a3x2  1+ata*a3x3  0  0  1]
  gama=[1,1,1,1,1,1]'
     C=[2.8306e+019   1.0675e+019  -5.3788e+019 0 0 0;
        5.0951e+019   1.9215e+019  -9.6819e+019 0 0 0;
        2.1654e+019   8.1665e+018  -4.1148e+019 0 0 0]
 xx(:,1)=[0,0,0,0,0,0]'
 z(1:3)=C*xx(:,1)+v(1:3)'
for i=2:Bushu-2
    xx(:,i)=fai*xx(:,i-1)+gama*w(i-1);
    z(i:i+2)=C*xx(:,i)+v(i:i+2)';
end
%***kalman滤波器***
n=6;
xjian(:,1)=zeros(6,1);p(:,:)=zeros(6);
for i=1:Bushu-1
xxjian(:,i+1)=fai*xjian(:,i);
e(:,i+1)=z(i+1)-C*xxjian(:,i+1);
pp(:,n*(i-1)+1:n*(i))=fai*p(:,n*(i-1)+1:n*i)*fai'+gama*Q*gama';
kf(:,i+1)=pp(:,n*(i-1)+1:n*i)*C'*inv(C*pp(:,n*(i-1)+1:n*i)*C'+R);
xjian(:,i+1)=xxjian(:,i+1)+kf(:,i+1)*e(:,i+1);
p(:,n*i+1:n*(i+1))=[eye(6)-kf(:,i+1)*C]*pp(:,n*(i-1)+1:n*i);
end
 xxjian
%***
t=1:Bushu;
subplot(2,2,1);plot(t,x(1,t),'b',t,xjian(1,t),'r:');
subplot(2,2,2);plot(t,x(2,t),'b',t,xjian(2,t),'r:');