%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% function call: out_data=GPSaidedINS(in_data,settings)
%
% This function integrates GNSS and IMU data. The data fusion
% is based upon a loosely-coupled feedback GNSS-aided INS approach.
%
% Input struct:
% in_data.GNSS.pos_ned      GNSS-receiver position estimates in NED
%                           coordinates [m]
% in_data.GNSS.t            Time of GNSS measurements [s]
% (in_data.GNSS.HDOP        GNSS Horizontal Dilution of Precision [-]) 
% (in_data.GNSS.VDOP        GNSS Vertical Dilution of Precision [-]) 
% in_data.IMU.acc           Accelerometer measurements [m/s^2]
% in_data.IMU.gyro          Gyroscope measurements [rad/s]
% in_data.IMU.t             Time of IMU measurements [s]
%
% Output struct:
% out_data.x_h              Estimated navigation state vector [position; velocity; attitude]
% out_data.delta_u_h        Estimated IMU biases [accelerometers; gyroscopes]
% out_data.diag_P           Diagonal elements of the Kalman filter state
%                           covariance matrix.
% out_data.energy            overall estimated power consumption
% out_data.error            average RMS
%
%
% Edit: Isaac Skog (skog@kth.se), 2016-09-06
% Revised: Bo Bernhardsson, 2018-01-01
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function out_data=GPSaidedINS(in_data,settings)


% Copy data to variables with shorter name
u=[in_data.IMU.acc;in_data.IMU.gyro];
t=in_data.IMU.t;

%% Information fusion
start_sim=822799;               %start index of time vector of IMU
length_sim=20000;
[is, where]=ismember(in_data.IMU.t(start_sim),in_data.GNSS.t); %find same start on GPS time vector
ctr_gnss_data=where;
GPS_position=true;

%% Initialization
% Initialize the navigation state
x_h=init_navigation_state(u,settings,in_data,where);

% Initialize the sensor bias estimate
delta_u_h=zeros(6,1);

% Initialize the Kalman filter
[P,Q1,Q2,~,~]=init_filter(settings);

% Allocate memory for the output data
N=size(u,2);
out_data.x_h=zeros(10,N);
out_data.x_h(:,1)=x_h;
out_data.diag_P=zeros(15,N);
out_data.diag_P(:,1)=diag(P);
out_data.delta_u_h=zeros(6,N);
turn_store=true;

%% Initialize GPS sensor 
sensor.state='warm_start_available';
sensor.fetchfp=1.0;
sensor.geteph=Inf;
sensor.ephExp=in_data.IMU.t(start_sim)+180000.0;
sensor.fp=0;
sensor.eph=5;
sv=5;                 %initialize the number of visible satellites
turn=true;
power=400;
energy=0;
P_treshold=settings.P_treshold;

pre_state='startup';  %initialize previous state of the sensor (used for displaying state changes) 
P_store=0;                  %store summed valued of the trace of P in time
sv_store=sv;          %store number of visible satellites in time

%for k=2:N
for k=start_sim:start_sim+length_sim

    % Sampling period
    Ts=0.01;%t(k)-t(k-1);
    
    %%%%%%%%%%%%%%%%%
    %% GPS sensor  %%
    %%%%%%%%%%%%%%%%%
    %randomized number of visible satellites
    %sv = random_satellites(sv);
    sv=5;
    %control action
    P_store=[P_store,sum(sqrt(out_data.diag_P(1:3,k-1)))];
    if sum(sqrt(out_data.diag_P(1:3,k-1)))<P_treshold
        turn=false;
    else
        turn=true;
    end
    %update sensor state
%     pre_state=sensor.state;
    sensor=gps_randomized(t(k),sensor,turn,sv); %remove _randomized for worst case
    %determine if GPS position is available
    GPS_position=strcmp(sensor.state,'position_available');
    %compute power consumption
    if ~(strcmp(sensor.state,'no_info')||strcmp(sensor.state,'warm_start_available'))
        energy=energy+power*Ts;
    end
    
%      %print new state and current time if it changes
%      if ~strcmp(pre_state, sensor.state)
%          sensor.state
%          t(k)
%      end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Calibrate the sensor measurements using current sensor bias estimate.
    u_h=u(:,k)+delta_u_h;
    
    % Update the INS navigation state
    x_h=Nav_eq(x_h,u_h,Ts);
    
    % Get state space model matrices
    [F,G]=state_space_model(x_h,u_h,Ts);
    
    % Time update of the Kalman filter state covariance.
    P=F*P*F'+G*blkdiag(Q1,Q2)*G';
    
    % Defualt measurement observation matrix  and measurement covariance
    % matrix
    
    y = in_data.GNSS.pos_ned(:,ctr_gnss_data);     
    H = [eye(3) zeros(3,12)]; 
    R = settings.sigma_gps^2*eye(3);
    
    ind = zeros(1,6);  % index vector, describing available measurements
    % Check if GNSS measurement is available
    if t(k)==in_data.GNSS.t(ctr_gnss_data) 
       if GPS_position
            ind(1:3)=1;
       end
        % Update GNSS data counter
        ctr_gnss_data=min(ctr_gnss_data+1,length(in_data.GNSS.t));
    end
    
    ind=logical(ind);
    H=H(ind,:);
    y=y(ind);
    R=R(ind,ind);
    
    % Calculate the Kalman filter gain.
    K=(P*H')/(H*P*H'+R);
    
    % Update the perturbation state estimate.
    z=[zeros(9,1); delta_u_h]+K*(y-H(:,1:6)*x_h(1:6));
    
    % Correct the navigation states using current perturbation estimates.
    x_h(1:6)=x_h(1:6)+z(1:6);
    x_h(7:10)=Gamma(x_h(7:10),z(7:9));
    delta_u_h=z(10:15);
    
    % Update the Kalman filter state covariance.
    P=(eye(15)-K*H)*P;
    
    % Save the data to the output data structure
    out_data.x_h(:,k)=x_h;
    out_data.diag_P(:,k)=diag(P);
    out_data.delta_u_h(:,k)=delta_u_h;
    out_data.sv(k)=sv;
    turn_store=[turn_store, turn];
end

%% plot feedback over P matrix
%figure(11)
%plot(P_store),grid

%% output for simulation of energy-accuracy tradeoff 
out_data.energy=energy;

xest = out_data.x_h(2,:);%start_sim:start_sim+length_sim);
yest = out_data.x_h(1,:);%start_sim:start_sim+length_sim);
xgps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(2,:),in_data.IMU.t,'linear','extrap')';
ygps = interp1(in_data.GNSS.t,in_data.GNSS.pos_ned(1,:),in_data.IMU.t,'linear','extrap')';
xerr = xest - xgps; 
yerr = yest - ygps;
out_data.error = sqrt(mean(xerr.^2+yerr.^2));
out_data.P_store=P_store;
out_data.turn=turn_store;
end






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                          SUB-FUNCTIONS                                %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%       Init filter          %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [P,Q1,Q2,R,H]=init_filter(settings)


% Kalman filter state matrix
P=zeros(15);
P(1:3,1:3)=settings.factp(1)^2*eye(3);
P(4:6,4:6)=settings.factp(2)^2*eye(3);
P(7:9,7:9)=diag(settings.factp(3:5)).^2;
P(10:12,10:12)=settings.factp(6)^2*eye(3);
P(13:15,13:15)=settings.factp(7)^2*eye(3);

% Process noise covariance
Q1=zeros(6);
Q1(1:3,1:3)=diag(settings.sigma_acc).^2;
Q1(4:6,4:6)=diag(settings.sigma_gyro).^2;

Q2=zeros(6);
Q2(1:3,1:3)=settings.sigma_acc_bias^2*eye(3);
Q2(4:6,4:6)=settings.sigma_gyro_bias^2*eye(3);

% GNSS-receiver position measurement noise
R=settings.sigma_gps^2*eye(3);

% Observation matrix
H=[eye(3) zeros(3,12)];

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Init navigation state     %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function x_h=init_navigation_state(u,settings,in_data,where)


% Calculate the roll and pitch
f=mean(u(:,1:100),2);
roll=atan2(-f(2),-f(3));
pitch=atan2(f(1),norm(f(2:3)));

% Initial coordinate rotation matrix
Rb2t=Rt2b([roll pitch settings.init_heading])';

% Calculate quaternions
q=dcm2q(Rb2t);

% Initial navigation state vector
x_h=[in_data.GNSS.pos_ned(:,where);zeros(3,1); q];
%x_h=[zeros(6,1); q];

end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  State transition matrix   %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [F,G]=state_space_model(x,u,Ts)

% Convert quaternion to DCM
Rb2t=q2dcm(x(7:10));

% Transform measured force to force in
% the tangent plane coordinate system.
f_t=Rb2t*u(1:3);
St=[0 -f_t(3) f_t(2); f_t(3) 0 -f_t(1); -f_t(2) f_t(1) 0];

% Only the standard errors included
O=zeros(3);
I=eye(3);
Fc=[O I O O O;
    O O St Rb2t O;
    O O O O -Rb2t;
    O O O O O;
    O O O O O];

% Approximation of the discret
% time state transition matrix
F=eye(15)+Ts*Fc;

% Noise gain matrix
G=Ts*[O O O O; Rb2t O O O; O -Rb2t O O; O O I O; O O O I];
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%     Error correction of quaternion    %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q=Gamma(q,epsilon)

% Convert quaternion to DCM
R=q2dcm(q);

% Construct skew symetric matrx
OMEGA=[0 -epsilon(3) epsilon(2); epsilon(3) 0 -epsilon(1); -epsilon(2) epsilon(1) 0];

% Correct the DCM matrix
R=(eye(3)-OMEGA)*R;

% Calculte the corrected quaternions
q=dcm2q(R);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%     Random evolution of visible satellites   %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function sv=random_satellites(sv_old)
coin=random('uniform',0,1);
sv=sv_old;
if sv_old==6  %simulate satellite loss of visibility
    if coin<0.005
        sv=5;
    end
end
if sv_old==5  %simulate satellite loss of visibility
    if coin>0.99
        sv=6;
    end
    if coin<0.001
        sv=4;
    end     
end
if sv_old==4  %simulate satellite loss of visibility
    if coin>0.99
        sv=5;
    end
    if coin<0.0005
        sv=3;
    end     
end
if sv_old==3  %simulate satellite loss of visibility
    if coin>0.95
        sv=4;
    end
end
end
