x1_position=[];
x1radar_position=[];
x1_0=[7700,5600,9700,180,40,55,0,0,0]';               %運動軌跡仿真初始值1
[x1radar_position,x1_position,x1_state]=track_cv(x1_0,1.5,60,0.2);   %目標1在[0,90s]作CV運動
x2Dradar_position=x1radar_position(1:2,:);
[x2radar_position,x2_position,x2_state]=track_cv(x1_0,1.5,60,0.0001);
xIF_position=x2radar_position(2:3,:);
%卡爾曼濾波
x=[7700,5600,9700,180,40,55]';
P=1000*[eye(6)];
F=[eye(3),diag([1.5 1.5 1.5]);
    zeros(3),eye(3)];
G=[diag([1.5 1.5 1.5]);
    eye(3)];
Cw=[diag([0.001^2 0.001^2 0.001^2])];
Cn=[10^2 0 0;0 0.05^2 0;0 0 0.05^2];
Cn1=[10^2 0 0;0 0.05^2 0];
Cn2=[0 0.05^2 0;0 0 0.05^2];
Fradar_position=[];

for i=1:60
    x1=F*x;
    P=F*P*F'+G*Cw*G';
    H=[x1(1)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) x1(2)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) x1(3)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) 0 0 0;
        -x1(2)/(x1(1)^2+x1(2)^2) x1(1)/(x1(1)^2+x1(2)^2) 0 0 0 0;];
    K=P*H'*inv(H*P*H'+Cn1);
    P=(eye(6)-K*H)*P;
    xtemp=x1+K*(x2Dradar_position(:,i)-[sqrt(x1(1)^2+x1(2)^2+x1(3)^2);atan(x1(2)/x1(1))]);
    H2=[-x1(2)/(x1(1)^2+x1(2)^2) x1(1)/(x1(1)^2+x1(2)^2) 0 0 0 0;
        -(x1(1)*x1(3))/(sqrt(x1(1)^2+x1(2)^2)*(x1(1)^2+x1(2)^2+x1(3)^2)) -(x1(2)*x1(3))/(sqrt(x1(1)^2+x1(2)^2)*(x1(1)^2+x1(2)^2+x1(3)^2)) sqrt(x1(1)^2+x1(2)^2)/(x1(1)^2+x1(2)^2+x1(3)^2) 0 0 0];
    K2=P*H2'*inv(H2*P*H2'+Cn2);
    xtemp=xtemp+K2*(xIF_position(:,i)-[atan(x1(2)/x1(1));atan(x1(3)/sqrt(x1()))]);
    
    
    Fradar_position=[Fradar_position x];
end