
% -----------------------------------
% ----> I. KALMAN FILTER FUSION <----
% -----------------------------------
% 1. System data matrices (those that characterize the system)
% Start with [TIME], [y_VARIABLE], [GPS_y_VARIABLE]
% STATE VECTOR: x_VECTOR = [y_VARIABLE  ydot_VARIABLE  ydot_bias]'
F=[0 1 0; 0 0 -1; 0 0 0]; %F=System dynamics (3x3) of an object with constant velocity
G=[0 1 0]'; % G=Sample noise ? (3x1)
QTUNE=10; %TUNE this
Q=(QTUNE)*[1 0 0; 0 1 0; 0 0 1]; % Q=Process noise covarience, Q=E[ww'] (3x3) R=[20]; %R=Measurement covarience (std^2), R=E[vv'] (1x1)  %TUNE this too :-) 
H=[1 0 0]; % Measurement update system matrix

% 2. PSI_k matrix 
SAMPLING_RATE=100; %sampling RATE of 100 Hz?
ts=1/SAMPLING_RATE; % Assign t
psik = [    1,  ts, -1/2*ts^2 ;
                0,   1, -ts ;
                0,   0,     1];
% Solved from:
% syms s
% psi_of_t=ilaplace(inv(s*eye(3)-F)); %psi(t)=invL[(sI-F)^-1]

% 3. Gk matrix (Gk=int (psi*G) dt)
Gk = [ 1/2*ts^2 ;   ts ;    0];
% Solved from:
% G1=psi_of_t*G;
% Gk=eval(int(G1));

% 4. Qk matrix (Qk=int (psi*Q*psi') dt)
Qk = [  QTUNE+ts^2*QTUNE+1/4*ts^4*QTUNE,  t*QTUNE+1/2*ts^3*QTUNE,  -1/2*ts^2*QTUNE;
        ts*QTUNE+1/2*ts^3*QTUNE,          QTUNE+ts^2*QTUNE,        -ts*QTUNE;
        -1/2*ts^2*QTUNE,                  -ts*QTUNE,                QTUNE];
%Solved from:
%psi_tras=ilaplace(inv(s*eye(3)-F')); %psi_of_t' gives conj(t) terms, which do not apply (real assump)
%Q1=psi_of_t*Q*psi_tras;
%Qk=eval(int(Q1, 0, ts));

% 5. Matrix allocation and initial value setting
numpts=length(y_VARIABLE);
x=zeros(3, numpts); %state vector x_VECTOR = [y_VARIABLE  ydot_VARIABLE  ydot_bias]'
xdot=zeros(3, numpts); %first derivative, xdot_VECTOR=[ydot ydotdot baisdot]'
Pk=zeros(3,3,numpts); %Pk(+)=Pk=P+  

% Set initial values (REMEMBER index is shiffted of by one)
x(:,1)=zeros(3,1); % x(0)=0 
%%xdot(:,1)=zeros(3,1); % xdot(0)=0 
Pk(:,:,1)=eye(3); %Pm(0)=I

% 6. Iterate
for k=2:numpts
    % Calculate
    Mk=psik*Pk(:,:,k-1)*psik' + Qk;
    
    Kk=Mk*H'*inv(H*Mk*H' + R);  

    Pkp=(eye(3)-Kk*H)*Mk;
    
    Term1=psik*x(:,k-1); 
    Term2=Gk*y_VARIABLE(k-1); 
    
    xhat= Term1 + Term2 + Kk*(GPS_y_VARIABLE(k)-H*Term1-H*Term2);
    
    %Store
    x(:,k)=xhat;
    Pk(:,:,k)=Pkp;
    
end