[MATLAB混合编程] KALMAN滤波的MATLAB程序出错，请帮忙调试

[dj2531]

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:');