function [plotP,truer,truev,r,v,T,tf] = EKF()
%  Extended Kalman Filter ,descrete and non-linear
%  Nov-29-2007
% Output:
% Input:
%      X0:the initial state vector
% Reference:
%          最佳估計理論 陳新海 編
%          第六章 非線性濾波 6.3推廣的kalman濾波
clc
clear
T = 3;% the interval time
tf = 45000;%the whole simulation time
X0 = [4.590e6,4.388e6,3.228e6,-4.612e3,5.014e2,5.876e3]';% initial true state;
N = length(X0);%get the length of A
P0 = diag([400^2,400^2,400^2,0.8^2,0.8^2,0.8^2]);
Q=diag([(1e-3)^2,(2e-3)^2,(1e-3)^2,(1e-3)^2,(2e-3)^2,(1e-3)^2]);%process noise covriance
Rnoise=80^2;%measurement noise covariance

%simulation the system
P = P0;
X = X0;
Z = [];
A = [];
[Z,truer,truev                                                                                                                                                                                     ] = GetZ(X0,T,tf,Q);
i = 1;


for t = 0:T:tf
%save X,P for drawing picture    
plotx(:,i) = X;
plotP(:,i) = diag(P);

r(i) = sqrt(X(1)^2+X(2)^2+X(3)^2);
v(i) = sqrt(X(4)^2+X(5)^2+X(6)^2);
% detr(i) = abs(truer(i)-r);
% detv(i) = abs(truev(i)-v);
% Pr(i) = 2*(sqrt(plotP(1,1))+sqrt(plotP(2,1))+sqrt(plotP(3,1)))/3;
% Pv(i) = 2*(sqrt(plotP(4,1))+sqrt(plotP(5,1))+sqrt(plotP(6,1)))/3;

A = GetA(X);
H = GetH(X);
Fi = eye(N) + A*T;
f = Getf(X);

Pmin = Fi*P*Fi'+Q;
K = Pmin*H'*inv(H*Pmin*H'+Rnoise);
Xmin = X+f*T+A*f*T^2/2;
h = Getlittleh(Xmin);
X = Xmin + K*(Z(i)-h);
P = (eye(N)-K*H)*Pmin;




i = i+1;
end
% figure;
% plot3(plotx(1,:),plotx(2,:),plotx(3,:));
% figure;
% plot3(ptruex(1,:),ptruex(2,:),ptruex(3,:));