function [nav_e] = ins_visual(imu, gnss, visual, att_mode)
% ins_visual: loosely-coupled integrated navigation system.
%
% ins_visual integrates IMU and visual measurements by using an Extended Kalman filter.
%
% INPUT
%   imu, IMU data structure.
%         t: Ix1 time vector (seconds).
%        fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
%        wb: Ix3 turn rates vector in body frame XYZ (radians/s).
%       arw: 1x3 angle random walks (rad/s/root-Hz).
%       vrw: 1x3 velocity random walks (m/s^2/root-Hz).
%     g_std: 1x3 gyros standard deviations (radians/s).
%     a_std: 1x3 accrs standard deviations (m/s^2).
%    gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
%    ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
%    gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
%    ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
%   gb_corr: 1x3 gyros correlation times (seconds).
%   ab_corr: 1x3 accrs correlation times (seconds).
%    gb_psd: 1x3 gyros dynamic biases root-PSD (rad/s/root-Hz).
%    ab_psd: 1x3 accrs dynamic biases root-PSD (m/s^2/root-Hz);
%      freq: 1x1 sampling frequency (Hz).
% ini_align: 1x3 initial attitude at t(1).
% ini_align_err: 1x3 initial attitude errors at t(1).
%
%	gnss, GNSS data structure.
%         t: Gx1 time vector (seconds).
%       lat: Gx1 latitude (radians).
%       lon: Gx1 longitude (radians).
%         h: Gx1 altitude (m).
%       vel: Gx3 NED velocities (m/s).
%       std: 1x3 position standard deviations (rad, rad, m).
%      stdm: 1x3 position standard deviations (m, m, m).
%      stdv: 1x3 velocity standard deviations (m/s).
%      larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
%      freq: 1x1 sampling frequency (Hz).
%   zupt_th: 1x1 ZUPT threshold (m/s).
%  zupt_win: 1x1 ZUPT time window (seconds).
%       eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
%
%   visual, visual data structure
%         t: Vx1 time vector (seconds)
%       lat: Vx1 latitude (radians).
%       lon: Vx1 longitude (radians).
%         h: Vx1 altitude (m).
%       vel: Vx3 NED velocities (m/s).
%     covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
%      covv: Vx9 velocity covariance matrices (m^2/s^2).
%      larm: 3x1 lever arm (m).
%      freq: 1x1 sampling frequency (Hz).
%       eps: 1x1 time interval to compare current IMU time to current visual time vector (s).
%
%  att_mode: attitude mode string.
%      'quaternion': attitude updated in quaternion format. Default value.
%             'dcm': attitude updated in Direct Cosine Matrix format.
%
% OUTPUT
%   nav_e, INS/visual navigation estimates data structure.
%         t: Ix1 INS time vector (seconds).
%        tg: Vx1 VISUAL time vector, when Kalman filter was executed (seconds).
%      roll: Ix1 roll (radians).
%     pitch: Ix1 pitch (radians).
%       yaw: Ix1 yaw (radians).
%       vel: Ix3 NED velocities (m/s).
%       lat: Ix1 latitude (radians).
%       lon: Ix1 longitude (radians).
%         h: Ix1 altitude (m).
%        xi: Vxn Kalman filter a priori states.
%        xp: Vxn Kalman filter a posteriori states.
%         z: Vxr INS/visual measurements
%         v: Vxr Kalman filter innovations.
%         b: Vxr Kalman filter biases compensations, [gb_dyn ab_dyn].
%         A: Vxn^2 Kalman filter transition-state matrices, one matrix per
%            row ordered by columns.
%        Pp: Vxn^2 Kalman filter a posteriori covariance matrices, one
%         matrix per row ordered by columns.
%        Pi: Vxn^2 Kalman filter a priori covariance matrices, one matrix
%            per row ordered by columns.
%         K: Vx(n*r) Kalman gain matrices
%         S: Vxr^2 Innovation matrices
%        ob: Vx1 Number of observable states after each visual data arriving
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Alg. 2.
%
%   Groves, P.D. (2013), Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
%
%   ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
% and Motion Constraints".
%
%   ins_gps.m, ins_gnss function is based on that previous NaveGo function.
%
% Version: 002
% Date:    2022/07/19
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

if nargin < 3, att_mode  = 'quaternion'; end

%% ZUPT ALGORITHM

zupt_flag = false;

%% PREALLOCATION

% Kalman filter dimensions
n = 15; % number of states
r = 6;  % number of sensors
q = 12; % number of inputs

% Constant matrices
I = eye(3);
O = zeros(3);

% Length of INS time vector
LI = length(imu.t);

% Length of visual time vector
LV = length(visual.t);

% Preallocation of attitude vectors
roll_e  = zeros (LI, 1);
pitch_e = zeros (LI, 1);
yaw_e   = zeros (LI, 1);

% Preallocation of velocity vector
vel_e   = zeros (LI, 3);

% Preallocation of gravity vector
gn_e   = zeros (LI, 3);

% Preallocation of position vectors
lat_e    = zeros (LI, 1);
lon_e    = zeros (LI, 1);
h_e      = zeros (LI, 1);

% Preallocation of Kalman filter matrices for later performance analysis
xi = zeros(LV, n);      % Evolution of Kalman filter a priori states
xp = zeros(LV, n);      % Evolution of Kalman filter a posteriori states
z = zeros(LV, r);       % INS/visual measurements
v = zeros(LV, r);       % Kalman filter innovations

A  = zeros(LV, n^2);    % Transition-state matrices
Pi = zeros(LV, n^2);    % A priori covariance matrices
Pp = zeros(LV, n^2);    % A posteriori covariance matrices
K  = zeros(LV, n*r);    % Kalman gain matrices
S  = zeros(LV, r^2);    % Innovation matrices
ob = zeros(LV, 1);      % Number of observable states at each visual data arriving

b = zeros(LV, r);       % Biases compensantions after Kalman filter correction

%% INITIAL VALUES AT INS TIME = 1

% Initial attitude
roll_e(1)  = imu.ini_align(1);
pitch_e(1) = imu.ini_align(2);
yaw_e(1)   = imu.ini_align(3);
DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
DCMbn = DCMnb';
qua   = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);

% Initial velocity
vel_e(1,:) = gnss.vel(1,:);

% Initial position
lat_e(1) = gnss.lat(1);
lon_e(1) = gnss.lon(1);
h_e(1)   = gnss.h(1);

% Initial dynamic biases
gb_dyn = imu.gb_dyn';
ab_dyn = imu.ab_dyn';

% Turn-rates update with both updated velocity and position
omega_ie_n = earth_rate(lat_e(1));
omega_en_n = transport_rate(lat_e(1), vel_e(1,1), vel_e(1,2), h_e(1));

% Gravity update
gn_e(1,:) = gravity(lat_e(1), h_e(1));

%% INITIALIZATION OF KALMAN FILTER MATRICES

% Prior estimates
kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]';  % Error vector state
kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);

kf.Q  = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);

fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta';
fn = DCMbn * fb_corrected;
wn_corrected = imu.wb(1,:)' - gb_dyn - imu.gb_sta';
wn = DCMbn * wn_corrected;

% Vector to update matrix F
upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];

% Update matrices F and G
[kf.F, kf.G] = F_update(upd, DCMbn, imu);

[RM,RN] = radius(gnss.lat(1));
Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]);  % radians-to-meters

% Update matrix H
kf.H = [ O I O O O ;
    O O Tpr O O ; ];
kf.R = diag([gnss.stdv gnss.stdm]).^2;
kf.z = [ gnss.stdv, gnss.stdm ]';

% Propagate prior estimates to get xp(1) and Pp(1)
kf = kf_update( kf );

% Initial matrices for Kalman filter performance analysis
xi(1,:) = kf.xi';
xp(1,:) = kf.xp';
Pi(1,:) = reshape(kf.Pi, 1, n^2);
Pp(1,:) = reshape(kf.Pp, 1, n^2);
K(1,:)  = reshape(kf.K, 1, n*r);
S(1,:)  = reshape(kf.S, 1, r^2);
v(1,:)  = kf.v';
z(1,:)  = kf.z';
b(1,:) = [gb_dyn', ab_dyn'];

%% INS (IMU) TIME IS THE MASTER CLOCK
for i = 2:LI

    %% INERTIAL NAVIGATION SYSTEM (INS)

    % Print a dot on console every 10,000 INS executions
    if (mod(i,10000) == 0), fprintf('. ');  end
    % Print a return on console every 200,000 INS executions
    if (mod(i,200000) == 0), fprintf('\n'); end

    % IMU sampling interval
    dti = imu.t(i) - imu.t(i-1);

    % Inertial sensors corrected with a posteriori KF biases estimation and
    % deterministic static biases
    wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
    fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
    fn = DCMbn * fb_corrected;
    wn = DCMbn * wb_corrected;

    % Velocity update
    vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i,:)', dti);
    vel_e (i,:) = vel;

    % Position update
    pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
    lat_e(i) = pos(1);
    lon_e(i) = pos(2);
    h_e(i)   = pos(3);

    % Turn-rates update with both updated velocity and position
    omega_ie_n = earth_rate(lat_e(i));
    omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));

    % Gravity update
    gn_e(i,:) = gravity(lat_e(i), h_e(i));
    
    % Attitude update
    [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
        omega_ie_n, omega_en_n, dti, att_mode);
    roll_e(i) = euler(1);
    pitch_e(i)= euler(2);
    yaw_e(i)  = euler(3);

    %% ZUPT DETECTION ALGORITHM
    idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time

    if ( i > idz )

        % Mean velocity value for the ZUPT window time
        vel_m = mean (vel_e(i-idz:i , :));

        % If mean velocity value is under the ZUPT threshold velocity...
        if (abs(vel_m) < gnss.zupt_th)
            disp("ZUPT condition applied");

            % Current attitude is equal to the mean of previous attitudes
            % inside the ZUPT window time
            roll_e(i)  = mean (roll_e(i-idz:i , :));
            pitch_e(i) = mean (pitch_e(i-idz:i , :));
            yaw_e(i)   = mean (yaw_e(i-idz:i , :));

            % Current position is equal to the mean of previous positions
            % inside the ZUPT window time
            lat_e(i) = mean (lat_e(i-idz:i , :));
            lon_e(i) = mean (lon_e(i-idz:i , :));
            h_e(i)   = mean (h_e(i-idz:i , :));

            % Alternative attitude ZUPT correction
            % roll_e(i)  = (roll_e(i-idz , :));
            % pitch_e(i) = (pitch_e(i-idz , :));
            % yaw_e(i)   = (yaw_e(i-idz, :));
            % lat_e(i) = (lat_e(i-idz:i , :));
            % lon_e(i) = (lon_e(i-idz:i , :));
            % h_e(i)   = (h_e(i-idz:i , :));

            zupt_flag = true;

            %             fprintf(' z\n')       % DEBUG
        end
    end

    %% KALMAN FILTER UPDATE

    % Check if there is a new visual measurement to process at current INS time
    gdx =  find (visual.t >= (imu.t(i) - visual.eps) & visual.t < (imu.t(i) + visual.eps));

    if (~isempty(gdx) && gdx(1) > 1)
        for index = 1:size(gdx,1)
            % disp("updated with visual measurements");

            %% MEASUREMENTS

            % Meridian and normal radii of curvature update
            [RM,RN] = radius(lat_e(i));

            % Radians-to-meters matrix
            Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);

            % Position innovations in meters with lever arm correction
            zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [visual.lat(gdx(index)); visual.lon(gdx(index)); visual.h(gdx(index));]) ...
                + (DCMbn * visual.larm);

            % Velocity innovations with lever arm correction
            zv = (vel_e(i,:) - visual.vel(gdx(index),:) - ((omega_ie_n + omega_en_n) * (DCMbn * visual.larm))' ...
                + (DCMbn * skewm(wb_corrected) * visual.larm )' )';

            %% KALMAN FILTER

            % visual sampling interval
            dtg = visual.t(gdx(index)) - visual.t(gdx(index)-1);

            % Vector to update matrix F
            upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];

            % Matrices F and G update
            [kf.F, kf.G] = F_update(upd, DCMbn, imu);

            % Matrix H update
            if(zupt_flag == false)
                kf.H = [ O I O O O ;
                    O O Tpr O O ; ];
                kf.R = reshape(visual.covvm(gdx(index),:),[6,6])';
                kf.z = [ zv' zp' ]';
            else
                kf.H = [ O I O O O ; ];
                kf.R = reshape(visual.covv(gdx(index),:),[3,3])';
                kf.z = zv;
            end

            % a posteriori states are forced to be zero (error-state approach)
            kf.xp = zeros(n , 1);
            % Execution of the extended Kalman filter
            kf = kalman(kf, dtg);

            %% OBSERVABILITY

            % Number the observable states at current GNSS time
            ob(gdx) = rank(obsv(kf.F, kf.H));

            %% INS/GNSS CORRECTIONS

            % Quaternion correction
            qua_skew = -skewm(qua(1:3));    % According to Crassidis, qua_skew should be
            % positive, but if positive NaveGo diverges.
            % Crassidis, Eq. A.174a
            Xi = [qua(4)*eye(3) + qua_skew; -qua(1:3)'];

            % Crassidis, Eq. 7.34
            qua = qua + 0.5 .* Xi * kf.xp(1:3);
            qua = qua / norm(qua);          % Brute-force normalization

            % DCM correction
            DCMbn = qua2dcm(qua);

            % Attitude correction, method 1
            %         euler = qua2euler(qua);
            %         roll_e(i) = euler(1);
            %         pitch_e(i)= euler(2);
            %         yaw_e(i)  = euler(3);

            % Attitude correction, method 2
            roll_e(i)  = roll_e(i)  - kf.xp(1);
            pitch_e(i) = pitch_e(i) - kf.xp(2);
            yaw_e(i)   = yaw_e(i)   - kf.xp(3);

            % Velocity correction
            vel_e(i,1) = vel_e(i,1) - kf.xp(4);
            vel_e(i,2) = vel_e(i,2) - kf.xp(5);
            vel_e(i,3) = vel_e(i,3) - kf.xp(6);

            % Position correction
            lat_e(i) = lat_e(i) - kf.xp(7);
            lon_e(i) = lon_e(i) - kf.xp(8);
            h_e(i)   = h_e(i)   - kf.xp(9);

            % Biases estimation
            gb_dyn   = -kf.xp(10:12);
            ab_dyn   = -kf.xp(13:15);

            % Matrices for later Kalman filter performance analysis
            xi(gdx(index),:) = kf.xi';
            xp(gdx(index),:) = kf.xp';
            b(gdx(index),:) = [gb_dyn', ab_dyn'];
            A(gdx(index),:)  = reshape(kf.A,  1, n^2);
            Pi(gdx(index),:) = reshape(kf.Pi, 1, n^2);
            Pp(gdx(index),:) = reshape(kf.Pp, 1, n^2);

            if(zupt_flag == false)
                v(gdx(index),:)  = kf.v';
                z(gdx(index),:)  = kf.z';
                K(gdx(index),:)  = reshape(kf.K, 1, n*r);
                S(gdx(index),:)  = reshape(kf.S, 1, r^2);
            else
                zupt_flag = false;
                z(gdx(index),:)  = [ kf.z' 0 0 0 ]';
                v(gdx(index),:)  = [ kf.v' 0 0 0 ]';
                K(gdx(index),1:n*3) = reshape(kf.K, 1, n*3);
                S(gdx(index),1:9)  = reshape(kf.S, 1, 3^2);
            end
        end
    end

    if imu.t(i) > max(visual.t)
        break;
    end
end

%% Summary from INS/visual integration

nav_e.t     = imu.t(1:i, :);    % INS time vector
nav_e.tg    = visual.t;         % visual time vector, which is the time vector when the Kalman filter was executed
nav_e.roll  = roll_e(1:i, :);   % Roll
nav_e.pitch = pitch_e(1:i, :);  % Pitch
nav_e.yaw   = yaw_e(1:i, :);    % Yaw
nav_e.vel   = vel_e(1:i, :);    % NED velocities
nav_e.lat   = lat_e(1:i, :);    % Latitude
nav_e.lon   = lon_e(1:i, :);    % Longitude
nav_e.h     = h_e(1:i, :);      % Altitude
nav_e.gn    = gn_e(1:i, :);     % Gravity estimation in the nav-frame.

nav_e.xi    = xi;       % A priori states
nav_e.xp    = xp;       % A posteriori states
nav_e.z     = z;        % INS/visual measurements
nav_e.v     = v;        % Kalman filter innovations
nav_e.b     = b;        % Biases compensations

nav_e.A     = A;        % Transition matrices
nav_e.Pi    = Pi;       % A priori covariance matrices
nav_e.Pp    = Pp;       % A posteriori covariance matrices
nav_e.K     = K;        % Kalman gain matrices
nav_e.S     = S;        % Innovation matrices
nav_e.ob    = ob;       % Number of observable states after each visual data arriving

fprintf('\n');
end
