%%%%%%%%%%%%%%%%%%%%%作者：戴丁樟；學(xué)號(hào)：04S005087%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
close all;
ci_R=0.1;
ci_B=0.01;
ci_U=0.0001;
T=1;  
A=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1];
N=100;
X=zeros(1,N);
Y=zeros(1,N);
VX=zeros(1,N);
VY=zeros(1,N);
i_X=zeros(1,N); 
i_Y=zeros(1,N);
i_VX=zeros(1,N);
i_VY=zeros(1,N);
S=[10,-5,-0.2,0.2]';
S1=[10,-5,-0.2,0.2]';
S2=zeros(4,1);
S3=[10,-5,-0.2,0.2]';
S4=zeros(4,1);
X(1)=10;
Y(1)=-5;
VX(1)=-0.2;
VY(1)=0.2;
i_X(1)=10;
i_Y(1)=-5;
i_VX(1)=-0.2;
i_VY(1)=0.2;

noiseu1=sqrt(ci_U)*randn(1,N+1);
randn('state',0);
noiseu2=sqrt(ci_U)*randn(1,N+1);
for t=1:1:N+1
    u(:,t)=[0,0,noiseu1(t),noiseu2(t)]';
end
for t=1:1:N
    S2=A*S1+u(:,t);
    X(t+1)=S2(1);    
    Y(t+1)=S2(2);
    VX(t+1)=S2(3);
    VY(t+1)=S2(4);
    S1=S2;
    S(:,t+1)=[X(t+1),Y(t+1),VX(t+1),VY(t+1)]';
    %真實(shí)軌跡
    S4=A*S3;
    i_X(t+1)=S4(1);
    i_Y(t+1)=S4(2);
    i_VX(t+1)=S4(3);
    i_VY(t+1)=S4(4);
    S3=S4;
    %理想軌跡
    
end

% %%%%%%%%%%%%%%%%%%%%%%%%上述程序求出了飛行器的真實(shí)運(yùn)動(dòng)曲線%%%%%%%%%%%%%%%%%%%%%%%%
% 
% %%%%%%%%%%%%%%%%%%%%%%%%  P矩陣表示濾波誤差方差陣，而PF矩陣表示一步預(yù)測(cè)誤差方差陣  %%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%%%%%%%  est_S表示濾波值,  est_SF表示狀態(tài)一步預(yù)測(cè)值               %%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%%%%%%%  ci_R表示R上噪聲的方差,ci_B表示方位上噪聲的方差,ci_U表示速度上的噪聲%%%%%%%

est_S(:,1)=[5,5,0,0]';
est_SF(:,1)=zeros(4,1);
P(:,:,1)=N*eye(4);
PF(:,:,1)=zeros(4,4);
H(:,:,1)=zeros(2,4);
K(:,:,1)=zeros(4,2);
Q=[0 0 0 0;
   0 0 0 0;
   0 0 ci_U 0;
   0 0 0 ci_U];
C=[ci_R 0;
   0 ci_B];
randn('state',0);
noiseR=sqrt(ci_R)*randn(1,N+1);
randn('state',0);
noiseB=sqrt(ci_B)*randn(1,N+1);
guan(:,1)=[sqrt(X(1)^2+Y(1)^2)+noiseR(1),atan2(Y(1),X(1))+noiseB(1)]';
xy_guan(1,1)=guan(1,1)*cos(guan(2,1));
xy_guan(2,1)=guan(1,1)*sin(guan(2,1));

for t=1:1:N
    est_SF(:,t+1)=A*est_S(:,t);
    PF(:,:,t+1)=A*P(:,:,t)*A'+Q;
    H(1,1,t+1)= est_SF(1,t+1)/sqrt( est_SF(1,t+1)^2+ est_SF(2,t+1)^2);
    H(1,2,t+1)= est_SF(2,t+1)/sqrt( est_SF(1,t+1)^2+ est_SF(2,t+1)^2);
    H(2,1,t+1)= -est_SF(2,t+1)/( est_SF(1,t+1)^2+ est_SF(2,t+1)^2);
    H(2,2,t+1)= est_SF(1,t+1)/( est_SF(1,t+1)^2+ est_SF(2,t+1)^2);
    H(1,3,t+1)=0;
    H(1,4,t+1)=0;
    H(2,3,t+1)=0;
    H(2,4,t+1)=0;
    guan(:,t+1)=[sqrt(X(t+1)^2+Y(t+1)^2)+noiseR(t+1),atan2(Y(t+1),X(t+1))+noiseB(t+1)]';
    est_guan(:,t+1)=[sqrt(est_SF(1,t+1)^2+est_SF(2,t+1)^2),atan2(est_SF(2,t+1),est_SF(1,t+1))]';
    K(:,:,t+1)= PF(:,:,t+1)*H(:,:,t+1)'*inv(C+H(:,:,t+1)*PF(:,:,t+1)*H(:,:,t+1)');
    est_S(:,t+1)=est_SF(:,t+1)+K(:,:,t+1)*(guan(:,t+1)-est_guan(:,t+1));
    P(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*PF(:,:,t+1);
    xy_guan(1,t+1)=guan(1,t+1)*cos(guan(2,t+1));
    xy_guan(2,t+1)=guan(1,t+1)*sin(guan(2,t+1));
    err_mse(t+1)=norm(P(:,:,t+1));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%繪圖%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure,
h1=plot(X,Y,i_X,i_Y,est_S(1,:),est_S(2,:),xy_guan(1,:),xy_guan(2,:));
legend(h1,'理想航跡','真實(shí)航跡','估計(jì)航跡','觀測(cè)航跡');

% for t=1:1:N
%     err_S(:,t+1)=est_S(:,t+1)-S(:,t+1);
%     err_mse(t+1)=norm(err_S(:,t+1));
% end
% figure,
% subplot(2,2,1),plot(1:N+1,err_S(1,:));
% subplot(2,2,2),plot(1:N+1,err_S(2,:));
% subplot(2,2,3),plot(1:N+1,err_S(3,:));
% subplot(2,2,4),plot(1:N+1,err_S(4,:));
figure,
plot(1:N+1,err_mse);