% function []=white_noise(T)
%%%定義可調(diào)白噪聲模型函數(shù)
% clear all
% clc
%%%初始值確定
%%%系統(tǒng)的狀態(tài)向量表示為X(k)=[x x' y y']'
% x0=[z12 (z12-z11)/T z22 (z22-z21)/T];       %%%初始值的確定；zij中i=1表示x方向的量測值，i=2表示y方向的量測值，j=k表示k時(shí)刻的量測數(shù)據(jù)
X0=[120000 -426 2000 0]';
r=sqrt(120426^2+2000^2);
a=0.0166;
d=100;
e=0.03;
T=1;
A=[cos(a) -r*sin(a)
   sin(a) r*cos(a)];                        %%%a表示方位角θ,r表示徑向距離
B=[d 0
   0 e];                                    %%%d代表徑向距離測量誤差的協(xié)方差，e代表方位角測量誤差的協(xié)方差
R=A*B*A';                                   %%%量測噪聲在直角坐標(biāo)系下的協(xié)方差
P_yuce=[R(1,1) R(1,1)/T R(1,2) R(1,2)/T
        R(1,1)/T 2*R(1,1)/T^2 R(1,2)/T 2*R(1,2)/T^2
        R(1,2) R(1,2)/T R(2,2) R(2,2)/T
        R(1,2)/T 2*R(1,2)/T^2 R(2,2)/T 2*R(2,2)/T^2];   %%%初始協(xié)方差確定

      
%%%初始條件確定
F=[1 T 0 0
   0 1 0 0
   0 0 1 T
   0 0 0 1];                                       %%%一步轉(zhuǎn)移矩陣
C=[1 0 0 0
   0 0 1 0];                                       %%%量測矩陣
% R=                                               %%%R值在求P_chushi值的時(shí)候已經(jīng)確定，為一2*2的對角矩陣
% Q_0=[T^5/20 T^4/8 0 0
%    T^4/8 T^3/3 0 0
%    0 0 T^5/20 T^4/8
%    0 0 T^4/8 T^3/3];                              %%%量測噪聲序列的協(xié)方差矩陣
Q_0=500*eye(4,4);
I=eye(4,4);
W1=100*randn(1,90);
W2=100*randn(1,90);
          
%%%算法
Q=Q_0;
X_yuce(:,1)=X0;
Y_max=5.07;
c=1e8;
N=90;
for k=1:N
    [x,y]=surrounding2;                       
    Z(:,k)=[x(k) y(k)]'+[W1(k) W2(k)]';        %%%此處調(diào)用函數(shù)，引入z(k)
    X_guji(:,k)=F*X_yuce(:,k);                 
    V=Z(:,k)-C*X_guji(:,k);                    %%%V表示新息
    P_guji=F*P_yuce*F'+Q;                      
    S=C*P_guji*C'+R;                           %%%S代表新息過程協(xié)方差矩陣
    Y=V'*inv(S)*V;                             
    if(Y>Y_max)                                %%%Y_max表示機(jī)動發(fā)生的判決門限值
        Q=c*Q;                                 %%%c為一常數(shù)
    else
        Q=Q_0;
    end
%   P_guji=F*P_yuce*F'+Q;
    K=P_guji*C'*inv(S);
    X_yuce(:,k+1)=X_guji(:,k)+K*V;
    P_yuce=(I-K*C)*P_guji;
end

plot(X_yuce(1,:),X_yuce(3,:))
hold on;
plot(Z(1,:),Z(2,:),'r')