function [Z,X_yuce]=kalman_fliter_nine(A1,A2,A3,N,T,ave,var)
%%%A1,A2及A3為極坐標下測量的前三組數據，都為1*3的矩陣，為確定卡爾曼濾波的初始值和初始協方差,ave和var分別為采樣間隔T1的均值和方差
% clear all;
% clc;

%%%初始條件給定
% T=1;
% N=100;
Nvar1=50;
Nvar2=60;
Nvar3=20;
Wvar1=100;
Wvar2=80;
Wvar3=50;
V1=sqrt(Nvar1)*randn(1,N);              %%%產生方差為Nvar1的正態分布的過程噪聲數據
V2=sqrt(Nvar2)*randn(1,N);              %%%產生方差為Nvar2的正態分布的過程噪聲數據
V3=sqrt(Nvar3)*randn(1,N);              %%%產生方差為Nvar3的正態分布的過程噪聲數據
W1=sqrt(Wvar1)*randn(1,N);              %%%產生方差為Wvar1的正態分布的量測噪聲數據
W2=sqrt(Wvar2)*randn(1,N);              %%%產生方差為Wvar2的正態分布的量測噪聲數據
W3=sqrt(Wvar3)*randn(1,N);              %%%產生方差為Wvar3的正態分布的量測噪聲數據
I=eye(9);


%%%初始狀態給定
F=[1 T T^2/2 0 0 0 0 0 0
   0 1 T 0 0 0 0 0 0
   0 0 1 0 0 0 0 0 0
   0 0 0 1 T T^2/2 0 0 0
   0 0 0 0 1 T 0 0 0
   0 0 0 0 0 1 0 0 0
   0 0 0 0 0 0 1 T T^2/2
   0 0 0 0 0 0 0 1 T
   0 0 0 0 0 0 0 0 1];                %%%系統方程的一步轉移矩陣
H=[T^2/2 0 0 
   T 0 0
   1 0 0
   0 T^2/2 0
   0 T 0
   0 1 0
   0 0 T^2/2
   0 0 T
   0 0 1];                            %%%系統方程的噪聲輸入矩陣
C=[1 0 0 0 0 0 0 0 0
   0 0 0 1 0 0 0 0 0
   0 0 0 0 0 0 1 0 0];                %%%量測方程的量測矩陣
%Q=7.5^2*eye(3,3);                    %%%過程噪聲的協方差矩陣，為方陣，可設為對角陣
Q=[50 0 0
   0 60 0
   0 0 20];
R=[90 0 0
   0 40 0
   0 0 20];                          %%%量測噪聲的協方差矩陣，為方陣，不一定為對角陣


% d=100;                            %%%d為徑向距離測量誤差的協方差
% e=0.001;                          %%%e為方位角測量誤差的協方差
% f=0.001;                          %%%f為俯仰角測量誤差的協方差

%%%%%此處調用軌跡函數的前三組數值
a1=A1(1);
a2=A1(2);
a3=A1(3);                         %%%a代表方位角φ
b1=A2(1);
b2=A2(2);
b3=A2(3);                         %%%b代表俯仰角θ
c1=A3(1);
c2=A3(2);
c3=A3(3);                         %%%c代表徑向距離r
% a1=1.5703;
% a2=1.5698;
% a3=1.5693;
% b1=0;
% b2=0;
% b3=0;
% c1=sqrt(2000^2+1);
% c2=sqrt(2000^2+4);
% c3=sqrt(2000^2+9);
%%%以上數值假設狀態向量[x x' x'' y y' y'' z z' z'']=[10 300 10 10 200 20 10 100 30]
%%%依據其前三組數值[10 10 10],[315 220 125],[630 450 270]通過坐標轉換而來

%%%系統的初始狀態
Z11=c1*cos(a1)*cos(b1);
Z21=c1*sin(a1)*cos(b1);
Z31=c1*sin(b1);

Z12=c2*cos(a2)*cos(b2);
Z22=c2*sin(a2)*cos(b2);
Z32=c2*sin(b2);

Z13=c3*cos(a3)*cos(b3);
Z23=c3*sin(a3)*cos(b3);
Z33=c3*sin(b3);

X0=[Z13
    (Z13-Z12)/T
    ((Z13-Z12)/T-(Z12-Z11)/T)/T
    Z23
    (Z23-Z22)/T
    ((Z23-Z22)/T-(Z22-Z21)/T)/T
    Z33
    (Z33-Z32)/T
    ((Z33-Z32)/T-(Z32-Z31)/T)/T];

% %%%初始協方差矩陣
% D=[d 0 0
%    0 e 0
%    0 0 f];
% 
% B1=[cos(a1)*cos(b1) -c1*sin(a1)*cos(b1) -c1*cos(a1)*sin(b1)
%    sin(a1)*cos(b1) c1*cos(a1)*cos(b1) -c1*sin(a1)*sin(b1)
%    sin(b1) 0 c1*cos(b1)];
% E1=B1*D*B1';
% 
% 
% B2=[cos(a2)*cos(b2) -c2*sin(a2)*cos(b2) -c2*cos(a2)*sin(b2)
%    sin(a2)*cos(b2) c2*cos(a2)*cos(b2) -c2*sin(a2)*sin(b2)
%    sin(b2) 0 c2*cos(b2)];
% E2=B2*D*B2';
% 
% 
% B3=[cos(a3)*cos(b3) -c3*sin(a3)*cos(b3) -c3*cos(a3)*sin(b3)
%    sin(a3)*cos(b3) c3*cos(a3)*cos(b3) -c3*sin(a3)*sin(b3)
%    sin(b3) 0 c3*cos(b3)];
% E3=B3*D*B3';
E1=R;
E2=R;
E3=R;


for i=1:3
    for j=1:3
        P_yuce(3*i-2:3*i,3*j-2:3*j)=[E3(i,j) E3(i,j)/T E3(i,j)/T^2
                                E3(i,j)/T (E3(i,j)+E2(i,j))/T^2 (E3(i,j)+2*E2(i,j))/T^3
                                E3(i,j)/T^2 (E3(i,j)+2*E2(i,j))/T^3 (E3(i,j)+4*E2(i,j)+E1(i,j))/T^4];
    end
end


%%%卡爾曼濾波算法%%%
X_chushi=X0;
X_yuce(:,1)=X0;

T1=ave+sqrt(var)*randn(1,N);
%%%產生均值為ave，方差為var的正態分布隨機矩陣，作為不斷變化的采樣間隔
t=0;

for k=1:N
%     X(:,k)=F*X_chushi+H*[V1(k),V2(k),V3(k)]';
%     Z(:,k)=C*X(:,k)+[W1(k),W2(k),W3(k)]';
    t=t+T1(1,k);                                               %%%每次循環調用一組數值，而不是全部的數值
    [x y z]=fuyang(0,2000,0,2400,15,100,1,3,30,0,t);           %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%此處調用函數軌跡
    Z(:,k)=[x y z]'+[W1(k),W2(k),W3(k)]';
    X_guji(:,k)=F*X_yuce(:,k);
    P_guji=F*P_yuce*F'+H*Q*H';
    a=Z(:,k)-C*X_guji(:,k);
    A=C*P_guji*C'+R;
    K=P_guji*C'*inv(A);
    X_yuce(:,k+1)=X_guji(:,k)+K*a;
    P_yuce=[I-K*C]*P_guji;
%     X_chushi=X(:,k);
end


%%%繪圖
% plot3(X(1,:),X(4,:),X(7,:))
% hold on;
% plot3(X_yuce(1,:),X_yuce(4,:),X_yuce(7,:),'r')
% subplot(3,1,1);
% plot(X_yuce(1,2:101)-X(1,:),'r')
% hold on;
% plot(X_yuce(4,2:101)-X(4,:),'-.')
% hold on;
% plot(X_yuce(7,2:101)-X(7,:),'g')
% hold on;
% subplot(3,1,2);
% plot(X_yuce(2,2:101)-X(2,:),'r')
% hold on;
% plot(X_yuce(5,2:101)-X(5,:),'-.')
% hold on;
% plot(X_yuce(8,2:101)-X(8,:),'g')
% subplot(3,1,3)
% plot(X_yuce(3,2:101)-X(3,:),'r')
% hold on;
% plot(X_yuce(6,2:101)-X(6,:),'-.')
% hold on;
% plot(X_yuce(9,2:101)-X(9,:),'g')