TF=100.;
SIGNOISE=300.;
X=0.;
Y=0.;
XH=1000.;
YH=2000.;
XR1=1000000.;
YR1=20000.*3280.;
XR2=50000000.;
YR2=20000.*3280.;
XR3=1000000000.;
YR3=20000.*3280.;
BIAS=10000.;
BIASH=0.;
PHIS=0.;
ORDER=3;
TS=1.;
T=0.;
S=0.;
H=.01;
PHI=zeros(ORDER,ORDER);
P=zeros(ORDER,ORDER);
IDNP=eye(ORDER);
Q=zeros(ORDER,ORDER);
RMAT=zeros(3,3);
Q(3,3)=PHIS*TS;
P(1,1)=1000.^2;
P(2,2)=2000.^2;
P(3,3)=(BIAS-BIASH)^2;
RMAT(1,1)=SIGNOISE^2;
RMAT(2,2)=SIGNOISE^2;
RMAT(3,3)=SIGNOISE^2;
count=0;
while T<=TF
XR1OLD=XR1;
XR2OLD=XR2;
XR3OLD=XR3;
XR1D=-14600.;
XR2D=-14600.;
XR3D=-14600.;
XR1=XR1+H*XR1D;
XR2=XR2+H*XR2D;
XR3=XR3+H*XR3D;
T=T+H;
XR1D=-14600.;
XR2D=-14600.;
XR3D=-14600.;
XR1=.5*(XR1OLD+XR1+H*XR1D);
XR2=.5*(XR2OLD+XR2+H*XR2D);
XR3=.5*(XR3OLD+XR3+H*XR3D);
S=S+H;
if S>=(TS-.00001)
S=0.;
R1H=sqrt((XR1-XH)^2+(YR1-YH)^2)+BIASH;
R2H=sqrt((XR2-XH)^2+(YR2-YH)^2)+BIASH;
R3H=sqrt((XR3-XH)^2+(YR3-YH)^2)+BIASH;
HMAT(1,1)=-(XR1-XH)/sqrt((XR1-XH)^2+(YR1-YH)^2);
HMAT(1,2)=-(YR1-YH)/sqrt((XR1-XH)^2+(YR1-YH)^2);
HMAT(1,3)=1.;
HMAT(2,1)=-(XR2-XH)/sqrt((XR2-XH)^2+(YR2-YH)^2);
HMAT(2,2)=-(YR2-YH)/sqrt((XR2-XH)^2+(YR2-YH)^2);
HMAT(2,3)=1.;
HMAT(3,1)=-(XR3-XH)/sqrt((XR3-XH)^2+(YR3-YH)^2);
HMAT(3,2)=-(YR3-YH)/sqrt((XR3-XH)^2+(YR3-YH)^2);
HMAT(3,3)=1.;
HT=HMAT';
PHI(1,1)=1.;
PHI(2,2)=1.;
PHI(3,3)=1.;
PHIT=PHI';
PHIP=PHI*P;
PHIPPHIT=PHIP*PHIT;
M=PHIPPHIT+Q;
HM=HMAT*M;
HMHT=HM*HT;
HMHTR=HMHT+RMAT;
HMHTRINV=inv(HMHTR);
MHT=M*HT;
GAIN=MHT*HMHTRINV;
KH=GAIN*HMAT;
IKH=IDNP-KH;
P=IKH*M;
R1NOISE=SIGNOISE*randn;
R2NOISE=SIGNOISE*randn;
R3NOISE=SIGNOISE*randn;
R1=sqrt((XR1-X)^2+(YR1-Y)^2);
R2=sqrt((XR2-X)^2+(YR2-Y)^2);
R3=sqrt((XR3-X)^2+(YR3-Y)^2);
RES1=R1+R1NOISE+BIAS-R1H;
RES2=R2+R2NOISE+BIAS-R2H;
RES3=R3+R3NOISE+BIAS-R3H;
XH=XH+GAIN(1,1)*RES1+GAIN(1,2)*RES2+GAIN(1,3)*RES3;
YH=YH+GAIN(2,1)*RES1+GAIN(2,2)*RES2+GAIN(2,3)*RES3;
BIASH=BIASH+GAIN(3,1)*RES1+GAIN(3,2)*RES2+GAIN(3,3)*RES3;
ERRX=X-XH;
SP11=sqrt(P(1,1));
ERRY=Y-YH;
SP22=sqrt(P(2,2));
ERRBIAS=BIAS-BIASH;
SP33=sqrt(P(3,3));
R1T=sqrt((XR1-X)^2+(YR1-Y)^2);
R2T=sqrt((XR2-X)^2+(YR2-Y)^2);
TEMP=(XR1-X)*(XR2-X)+(YR1-Y)*(YR2-Y);
TH1=atan2(sqrt((R1T*R2T)^2-TEMP^2),TEMP);
THET1=57.3*TH1;
R3T=sqrt((XR3-X)^2+(YR3-Y)^2);
TEMP3=(XR2-X)*(XR3-X)+(YR2-Y)*(YR3-Y);
TH2=atan2(sqrt((R2T*R3T)^2-TEMP3^2),TEMP3);
THET2=57.3*TH2;
SP11P=-SP11;
SP22P=-SP22;
SP33P=-SP33;
count=count+1;
ArrayT(count)=T;
ArrayX(count)=X;
ArrayXH(count)=XH;
ArrayY(count)=Y;
ArrayYH(count)=YH;
ArrayBIAS(count)=BIAS;
ArrayBIASH(count)=BIASH;
ArrayERRX(count)=ERRX;
ArraySP11(count)=SP11;
ArraySP11P(count)=SP11P;
ArrayERRY(count)=ERRY;
ArraySP22(count)=SP22;
ArraySP22P(count)=SP22P;
ArrayERRBIAS(count)=ERRBIAS;
ArraySP33(count)=SP33;
ArraySP33P(count)=SP33P;
end
end
% set(get(gca,'title'),'fontname','宋体')
figure
plot(ArrayT,ArrayBIAS,ArrayT,ArrayBIASH),grid
xlabel('时间 (Sec)','fontname','宋体')
ylabel('偏差 (Ft)','fontname','宋体')
axis([0 100 9000 11000])
figure
plot(ArrayT,ArrayX,ArrayT,ArrayXH),grid
xlabel('时间 (Sec)','fontname','宋体')
ylabel('接收器的水平距离 (Ft)','fontname','宋体')
axis([0 100 -200 800])
figure
plot(ArrayT,ArrayY,ArrayT,ArrayYH),grid
xlabel('时间 (Sec)','fontname','宋体')
ylabel('接收器的高度 (Ft)','fontname','宋体')
axis([0 100 -200 1200])
figure
plot(ArrayT,ArrayERRX,ArrayT,ArraySP11,ArrayT,ArraySP11P),grid
xlabel('时间 (Sec)','fontname','宋体')
ylabel('水平距离估计的误差 (Ft)','fontname','宋体')
axis([0 100 -700 700])
figure
plot(ArrayT,ArrayERRY,ArrayT,ArraySP22,ArrayT,ArraySP22P),grid
xlabel('时间 (Sec)','fontname','宋体')
ylabel('高度估计的误差 (Ft)','fontname','宋体')
axis([0 100 -500 500])
figure
plot(ArrayT,ArrayERRBIAS,ArrayT,ArraySP33,ArrayT,ArraySP33P),grid
xlabel('时间 (Sec)','fontname','宋体')
ylabel('偏差估计的误差 (Ft)','fontname','宋体')
axis([0 100 -500 500])
clc
output=[ArrayT',ArrayX',ArrayXH',ArrayY',ArrayYH',ArrayBIAS',ArrayBIASH'];
save datfil.txt output -ascii
output=[ArrayT',ArrayERRX',ArraySP11',ArraySP11P',ArrayERRY',ArraySP22',...
ArraySP22P',ArrayERRBIAS',ArraySP33',ArraySP33P'];
save covfil.txt output -ascii
disp 'simulation finished'
