clear;
e=0.005;%xx.yy.
%e2=0.006白噪聲的均方誤差
W=[1 1 0 0;0 1 0 0;0 0 1 1;0 0 0 1];%狀態轉移矩陣
G=[0 0.5 0 0;0 1 0 0;0 0 0.5 0;0 0 1 0];%模型噪聲對狀態估計影響的轉移矩陣
Q=[0.25 0.5 0 0;0.5 1 0 0;0 0 0.25 0.5;0 0 0.5 1]*(e^2);%輸入擾動的協方差矩陣
R=[e^2,0;0,e^2];%觀測噪聲協方差矩陣

%獲得觀測值
for k=1:3000
x(k)=k;%實際值
%y(k)=0;%實際值
y(k)=k;%實際值
a(k)=atan(y(k)/x(k))+normrnd(0,e,1);%觀測角度值
r(k)=sqrt(x(k)^2+y(k)^2)+normrnd(0,e,1);%觀測距離值

%a(k)=atan(yy(k)/xx(k));%觀測值
%r(k)=sqrt(xx(k)^2+yy(k)^2);%觀測值
yy(k)=r(k)*sin(a(k));%轉化到直角坐標系下的觀測值
xx(k)=r(k)*cos(a(k));%轉化到直角坐標系下的觀測值
end


%兩點啟動
x0=[r(2)*cos(a(2));r(2)*cos(a(2))-r(1)*cos(a(1));r(2)*sin(a(2));r(2)*sin(a(2))-r(1)*sin(a(1))];%航跡初始的狀態估計
p1=[e^2 e^2 0 0;e^2 (e^2)*3 0 0;0 0 e^2 e^2;0 0 e^2 3*(e^2)];%初始狀態協方差矩陣
%p1=[1 0 0 0;0 1 0 0;0 0 0.01 0;0 0 0 0.01];
%rr=sqrt(x0(1)^2+x0(2)^2);
%H=[-x0(2)/(rr^2) 0  x0(1)/(rr^2)  0;x0(1)/rr 0 x0(2)/rr  0];%線性化


z=[r;a];
xxxx=W*x0;%預測值 
for k=3:3000   
    rr=sqrt(yy(k)^2+xx(k)^2);
    H=[xx(k)/rr 0 yy(k)/rr  0;-yy(k)/(rr^2) 0  xx(k)/(rr^2)  0];%線性化
  %z1(:,k)=z(:,k)+normrnd(0,0.005,2,1);
  p2=W*p1*W'+G*Q*G';
  %  p2=W*p1*W'+G*Q*G';%預測協方差陣
    
% s=H*p2*(H')+R
    K=p2*(H')*(inv(H*p2*(H')+R));%濾波增益陣
    p1=[eye(4)-K*H]*p2;%估計協方差
    %zz=H*xxxx
    zz=[sqrt((xxxx(1)^2+xxxx(3)^2));atan(xxxx(3)/xxxx(1))];
    %e=z1(:,k)-zz;
    E=z(:,k)-zz;
  K*E;
    xxx=xxxx+K*E;%估計值
    i(k)=xxx(1);
    j(k)=xxx(3);
    %x0=W*xxx;%預測值 
    xxxx=W*xxx;%預測值 
   
    ii(k)=xxxx(1);
    jj(k)=xxxx(3);
end


subplot(2,1,1);
plot(x,y,'r',i,j,'g')
%plot(x,y,'r',ii,jj,'g')

%plot(xx,yy)
subplot(2,1,2);
plot(x-i);,
%plot(i(3:10),j(3:10),'r',ii(3:10),jj(3:10),'g')
%plot(xx,yy,'r',x,y,'g')