% Simulation of a IMU/Lighthouse system.
%
% A ground truth trajectory is simulated.
%
% Simulation environment:
% -	IMU (with the characteristics of the MPU9150) and 240 Hz update rate)
% -	IMU Accelerometer spectral noise density: 0.008 m/s^2/sqrt(Hz)
% -	IMU Accelerometer bias random walk: 0.001 m/s^2/sqrt(s)
% -	One sensor
% -	Two base stations
% -	Lighthouse update rate 60 Hz
% -	Lighthouse angle measurement white noise: 65*1e-6 rad
%
% After 60 seconds, base station 2 is disabled. So we have only one sensor and
% one lighthouse. The total simulation time is 240 seconds (but this can go on
% for > 1h).  The user position is orbiting around location [4;4] with a
% velocity of about 0.3 – 0.5 m/s (figure 10). The object tracking is not lost,
% even with only one sensor and one base station due to the modelling of the
% system dynamics. As long as the Kalman filter prediction is corrected by
% single angle measurements, the tracking works. But it is not very accurate.
%
% Original reddit comment:
% https://www.reddit.com/r/oculus/comments/35kymh/valves_alan_yates_tweets_a_picture_of_the_very/

% It can actually (poorly) track a moving object with just one sensor and one
% base station...  We didn't expect this at all, it is not immediately
% intuitive, but even 2D simulations display this behavior. If you can tell me
% why, really why, you should apply for a job at Valve. :)
%
% Coordinate system
%
%     ^ North (x)
%     |
%     |
%     |
%     +-------------> East (y)
%
%
% Angles from LH base stations (N = user position, LH = lighthouse) This
% assumes all lighthouse positions are known in a local coordinate system. Also
% it is assumed that each lighthouse orientation unknown is calibrated, so we
% get true azimuth observations (in the local system).
%
%
%            de
%        +--------+ N
%        |       /
%        |      /
%        |     /
%     dn |    /
%        |phi/
%        |  /
%        | /
%        |/
%        +
%        LH
%

function [] = lighthouse()
% This is the main simulation function to produce a ground truth
% trajectory and noisy measurements for the solver/filter part.

close all; % close all figures
dbstop if error; % debugger break on error
rng(9001); % same random numbers each run

% Table of lighthouse positions.
%        North [m],  East [m]
lh_pos = [    1.00,  1.00 ; ...
              6.00   6.00 ];

% Sensor noise model
% Simulation base for the IMU:
% Typical values for the InvenSense MPU9150 in an indoor scenario.
imu_acc_bias  = [0.13;-0.08]; % initial IMU accelerometer bias (m/s^2)
imu_acc_psd   = [0.008; 0.008]; % IMU spectral noise density (m/s^2)/sqrt(Hz)
imu_acc_brw   = [0.001; 0.001]; % IMU bias random walk (m/s^2)/sqrt(s)
lh_sigma_rad  = 65*1e-6; % lighthouse 1 sigma angle white noise (rad)

% Update rates
dt_lh_sec     = 1/60; % 60 Hz lighthouse update frequency
dt_imu_sec    = 1/240; % 240 Hz IMU update frequency
dt_sim_sec    = dt_imu_sec/2; % simulation update dt
dt_eps        = dt_sim_sec/10; % epsilon for time steps

% Circle trajectory model
center        = [4; 4]; % center of simulated circle trajectory [m]
radius_m      = 2.0; % radius of circle trajectory [m]
speed_m_s     = 0.3; % initial speed on circle [m/s]
seconds       = 240; % total simulation time (sec)

vel_x         = [0;-speed_m_s]; % initial velocity
pos_x         = center + [radius_m; 0]; % initial position [m]
pos_x_min_1   = pos_x - vel_x*dt_sim_sec; % previous position

time_sec      = 0.000; % simulation time
time_imu_sec  = -1.000; % last IMU update time [seconds]
time_lh_sec   = -1.000; % last lighthouse update time [seconds]
epochs        = round(seconds / dt_sim_sec); % total simulation epochs
imu_epoch     = 1; % incremented for every IMU measurement
imu_epochs    = round(seconds / dt_imu_sec); % total IMU epochs
% Big arrays to generate plots at the end
history_x_tru = zeros(epochs, 3); % time, north, east (ground truth)
history_x_est = zeros(imu_epochs, 3); % time, n, e (estimated trajectory)
history_x_err = zeros(imu_epochs, 5); % time, n, e, sigma_n, sigma_e (true error)
history_b_tru = zeros(imu_epochs, 3); % time, true accel bias x, y
history_b_est = zeros(imu_epochs, 5); % time, est. accel bias (col 4,5: sigma)

% state machine to init kalman filter.
% we need two position estimations
KALMAN_STATE_LOOKING_FOR_INIT_POS = 0;
KALMAN_STATE_READY                = 1;
kalman_init_state = KALMAN_STATE_LOOKING_FOR_INIT_POS;

% -----------------------------------------------------------------------------
% SIMULATION
% -----------------------------------------------------------------------------

% Main simulation loop
for i=1:epochs
    history_x_tru(i, :) = [time_sec, pos_x'];

    % current acceleration (orbit around center)
    acc = (center - pos_x)*0.08;

    % Lighthouse
    % ----------
    if (time_sec - time_lh_sec >= dt_lh_sec - dt_eps)
        % Generate measurements
        angles = lh_angles(lh_pos, pos_x, lh_sigma_rad);

        % try to estimate an initial position to start the kalman filter
        if (kalman_init_state == KALMAN_STATE_LOOKING_FOR_INIT_POS)
            [estpos_x, sol_found] = twopointSolver(lh_pos(1,1), lh_pos(1,2), ...
                                                   lh_pos(2,1), lh_pos(2,2), ...
                                                   angles(1), angles(2));
            if (sol_found)
                fprintf('Found initial position: %.3f %.3f\n', ...
                    estpos_x(1), estpos_x(2));
                imu_acc_rand_walk = imu_acc_brw*sqrt(dt_imu_sec);
                estvel_x = [0;0]; % no velocity information
                kalman_state = kalman_init(estpos_x, estvel_x, imu_acc_rand_walk);
                kalman_init_state = KALMAN_STATE_READY;
            end
        end

        if (time_sec > 60)
            angles = angles(1); % only allow 1 lighthouse now
        end

        % Update estimator
        if (kalman_init_state == KALMAN_STATE_READY)
            kalman_state = kalman_fusion(kalman_state, lh_pos, angles, lh_sigma_rad);
        end
        time_lh_sec = time_sec;
    end

    % simulated IMU
    % -------------
    if (time_sec - time_imu_sec >= dt_imu_sec - dt_eps)
        % Generate IMU measurements
        history_b_tru(imu_epoch, :) = [time_sec, imu_acc_bias']; % store true bias (plot)
        % add bias and white noise to IMU measurement
        acc_imu = acc + imu_acc_bias + randn(2,1).*imu_acc_psd*sqrt(dt_imu_sec);
        % IMU bias random walk
        imu_acc_bias = imu_acc_bias + randn(2,1).*imu_acc_brw*sqrt(dt_imu_sec);

        % Update predictor
        if (kalman_init_state == KALMAN_STATE_READY)
            history_x_est(imu_epoch, :) = [time_sec, kalman_state.x(1:2)']; % store estimated pos (plot)
            history_x_err(imu_epoch, 1:3) = [time_sec, kalman_state.x(1:2)' - pos_x']; % store trajectory error (plot)
            history_x_err(imu_epoch, 4:5) = [kalman_state.Qxx(1,1)^(0.5), ...
                                             kalman_state.Qxx(2,2)^(0.5)]; % store 1 sigma (plot)
            history_b_est(imu_epoch, 1) = time_sec;
            history_b_est(imu_epoch, 2:3) = kalman_state.x(5:6)'; % store estimated bias (plot)
            history_b_est(imu_epoch, 4:5) = [kalman_state.Qxx(5,5)^(0.5), ...
                                             kalman_state.Qxx(6,6)^(0.5)]; % store 1 sigma (plot)
            sigma_acc = imu_acc_psd*sqrt(dt_imu_sec);
            kalman_state = kalman_predict(kalman_state, acc_imu, sigma_acc, dt_imu_sec);
        end
        imu_epoch = imu_epoch + 1;
        time_imu_sec = time_sec;
    end

    % Verlet integration for the next epoch
    tmp = 2*pos_x - pos_x_min_1 + acc*dt_sim_sec*dt_sim_sec;
    pos_x_min_1 = pos_x;
    pos_x = tmp;
    % fprintf('Velocity: %.3f\n', norm(pos_x - pos_x_min_1)/dt_sim_sec);

    time_sec = time_sec + dt_sim_sec;
end

% -----------------------------------------------------------------------------
% PLOT RESULTS
% -----------------------------------------------------------------------------

plotstep = 20;

% Plot accelerometer bias (north axis) (true value and estimated)
figure(1);
clf;
subplot(2,1,1);
hold on;
title(sprintf('Bias Random Walk X: %.3f$\\,\\frac{m/s^2}{\\sqrt{s}}$', imu_acc_brw(1)), 'interpreter','latex');
xlabel('Time [s]');
ylabel('Bias X Axis [m/s^2]');
plot(history_b_tru(1:plotstep:end,1), history_b_tru(1:plotstep:end,2), '-.k', 'LineWidth', 2);
plot(history_b_est(1:plotstep:end,1), history_b_est(1:plotstep:end,2), 'r', 'LineWidth', 2);
plot(history_b_est(1:plotstep:end,1), history_b_est(1:plotstep:end,2)+history_b_est(1:plotstep:end,4), 'b', 'LineStyle', '--');
legend('True bias', 'Est. bias', '1 \sigma band', 'Location','southeast');
plot(history_b_est(1:plotstep:end,1), history_b_est(1:plotstep:end,2)-history_b_est(1:plotstep:end,4), 'b', 'LineStyle', '--');

% Plot accelerometer bias (east axis) (true value and estimated)
subplot(2,1,2);
hold on;
xlabel('Time [s]');
ylabel('Bias Y Axis [m/s^2]');
title(sprintf('Bias Random Walk Y: %.3f$\\,\\frac{m/s^2}{\\sqrt{s}}$', imu_acc_brw(2)), 'interpreter','latex');
plot(history_b_tru(1:plotstep:end,1), history_b_tru(1:plotstep:end,3), '-.k', 'LineWidth', 2);
plot(history_b_est(1:plotstep:end,1), history_b_est(1:plotstep:end,3), 'r', 'LineWidth', 2);
plot(history_b_est(1:plotstep:end,1), history_b_est(1:plotstep:end,3)+history_b_est(1:plotstep:end,5), 'b', 'LineStyle', '--');
legend('True bias', 'Est. bias', '1 \sigma band', 'Location','northeast');
plot(history_b_est(1:plotstep:end,1), history_b_est(1:plotstep:end,3)-history_b_est(1:plotstep:end,5), 'b', 'LineStyle', '--');

% Plot trajectory North error
figure(2);
clf;
subplot(2,1,1);
hold on;
xlabel('Time [s]');
ylabel('Error North [m]');
plot(history_x_err(1:plotstep:end,1), history_x_err(1:plotstep:end,2), 'k', 'LineWidth', 2);
plot(history_x_err(1:plotstep:end,1), history_x_err(1:plotstep:end,4), 'b', 'LineStyle', '--');
legend('True error', '1-\sigma_{North}', 'Location','northwest');

subplot(2,1,2);
hold on;
xlabel('Time [s]');
ylabel('Error East [m]');
plot(history_x_err(1:plotstep:end,1), history_x_err(1:plotstep:end,3), 'k', 'LineWidth', 2);
plot(history_x_err(1:plotstep:end,1), history_x_err(1:plotstep:end,5), 'b', 'LineStyle', '--');
legend('True error', '1-\sigma_{East}', 'Location','northwest');

% Figure 10 plot north/east 2D trajectory plot
% with ground truth in black, estimated in red.
% Lighthouses are added as triangles.
figure(10);
clf;
hold on;
ms = 2; % map space
xlim([-ms 5+ms]);
ylim([-ms 5+ms]);
xlabel('East [m]');
ylabel('North [m]');

scatter(lh_pos(:,2), lh_pos(:,1), 'r^'); % plot lighthouse table
for i=1:size(lh_pos, 1)
    text(lh_pos(i,2)+0.2, lh_pos(i,1)+0.2, num2str(i)); % add lh number
end

% ground truth and estimated trajectory
plot(history_x_tru(1:plotstep*10:end,3), history_x_tru(1:plotstep*10:end,2), 'b*', 'LineStyle', 'none');
plot(history_x_est(1:plotstep*10:end,3), history_x_est(1:plotstep*10:end,2), 'o', 'LineStyle', 'none', 'MarkerEdgeColor',[1 0.85 0]);
legend('Lighthouse', 'Ground truth', 'Estimated','Location','northwest');
plot(history_x_tru(1:plotstep:end,3), history_x_tru(1:plotstep:end,2), 'b', 'LineWidth', 2);
plot(history_x_est(1:plotstep:end,3), history_x_est(1:plotstep:end,2), 'Color',[1 0.75 0]);

end

% @brief Initialize the Kalman filter struct.
% Input:
% @param[in] pos_x Best guess initial position
% @param[in] vel_x Best guess initial velocity
% @param[in] sigma_bias_rw Bias random walk (unit m/s^2, in discrete time)
% @return kalman_state Kalman filter state
function [kalman_state] = kalman_init(pos_x, vel_x, sigma_bias_rw)

% Kalman state:
% Position North, Position East, Velocity North, Velocity East
% n,e,v_n,v_e, acc_bias_n, acc_bias_e
kalman_state.unknowns = 6;
kalman_state.x = [pos_x;vel_x; 0; 0];
sigma_init_pos = 0.10; % 1 sigma initial position precision
sigma_init_vel = 1.0; % 1 sigma initial velocity precision
sigma_init_bias = 0.3; % 1 sigma initial accel bias precision
kalman_state.Qxx = blkdiag(eye(2)*sigma_init_pos^2, ...
                           eye(2)*sigma_init_vel^2, ...
                           eye(2)*sigma_init_bias^2);
kalman_state.prediction_noise = blkdiag(eye(2)*0.0, ...
                                        eye(2)*0.0, ...
                                        diag(sigma_bias_rw)^2);

end

% @brief Perform a Kalman Filter prediction step.
% Take the current accelerometer measurements to predict the state.
% Input:
% @param[in] kalman_state Current Kalman filter state
% @param[in] acc Current accelerometer measurement
% @param[in] sigma_acc White noise of the accelerometer measurements
% @return kalman_state Predicted Kalman filter state
function [kalman_state] = kalman_predict(kalman_state, acc, sigma_acc, dt_sec)

F = [ eye(2)   eye(2)*dt_sec -eye(2)*0.5*dt_sec^2; ...
      zeros(2) eye(2)        -eye(2)*dt_sec; ...
      zeros(2) zeros(2)       eye(2)];

B = [eye(2)*0.5*dt_sec^2; ...
     eye(2)*dt_sec; ...
     zeros(2)];

kalman_state.x = F*kalman_state.x + B*acc;

Qll = diag(sigma_acc)^2;
kalman_state.Qxx = F*kalman_state.Qxx*F' + ...
                   B*Qll*B' + ...
                   kalman_state.prediction_noise;

end

% @brief Perform a Kalman Filter (EKF) fusion step.
% Take the current lighthouse angular measurements and correct
% the estimated state.
% Input:
% @param[in] kalman_state Current Kalman filter state
% @param[in] lh_pos Position of the lighthouse basestations
% @param[in] angles Angular measurements from the lighthouse system
% @param[in] sigma_rad White noise of the angular measurements
% @return kalman_state Updated Kalman filter state
function [kalman_state] = kalman_fusion(kalman_state, lh_pos, angles, sigma_rad)

lh_count = size(angles,1);
A = zeros(lh_count, kalman_state.unknowns);
l = zeros(lh_count, 1);
Qll = eye(lh_count)*sigma_rad^2;
kalman_Qxx_start = kalman_state.Qxx;

for j=1:4 % iterations EKF
    est_pos = kalman_state.x(1:2);
    for i=1:lh_count
        dn = lh_pos(i, 1) - est_pos(1); % delta north
        de = lh_pos(i, 2) - est_pos(2); % delta east
        A(i, 1) =  de/(de^2 + dn^2); % dl/dn
        A(i, 2) = -dn/(de^2 + dn^2); % dl/de
        expected_phi = lh_angle(lh_pos(i,1), lh_pos(i,2), est_pos(1), est_pos(2));
        l(i) = wrapToPi(angles(i) - expected_phi);
    end
    S = A*kalman_Qxx_start*A' + Qll;
    K = (kalman_Qxx_start*A')/(S);
    dx = K*l;
    kalman_state.x = kalman_state.x + dx;
end

kalman_state.Qxx = (eye(kalman_state.unknowns) - K*A)*kalman_Qxx_start;
kalman_state.Qxx = 0.5*(kalman_state.Qxx + kalman_state.Qxx');

end

% @brief Least squares solver for a single epoch.
% Try to find the position based on one sensor
% and several lighthouse measurements.
% @param[in] lh_pos Lighthouse positions
% @param[in] angles Angle measurements
% @param[in] est_pos Estimated position.
% @return est_pos least squares solution
function [est_pos] = lsq_est_pos(lh_pos, angles, est_pos)

lh_count = size(lh_pos,1); % number of measurement
A = zeros(lh_count, 2); % Design matrix (Jacobi)
l = zeros(lh_count, 1); % measurement vector

for j=1:5 % iterations for non-linear least-squares
    % fill Jacobi matrix
    for i=1:lh_count
        de = lh_pos(i, 2) - est_pos(2);
        dn = lh_pos(i, 1) - est_pos(1);
        A(i, 1) =  de/(de^2 + dn^2); % dl/dn
        A(i, 2) = -dn/(de^2 + dn^2); % dl/de
        expected_phi = lh_angle(lh_pos(i,1), lh_pos(i,2), est_pos(1), est_pos(2));
        l(i) = angles(i) - expected_phi;
    end
    l = wrapToPi(l);
    dx = (A'*A)\(A'*l); % least squares adjustment
    est_pos = est_pos + dx;
end

end

% @brief Calculate the angles measured by the lighthouse
% system. Assumes the we can get all angles at the same epoch.
% (de = delta east, dn = delta north)
%
%            de
%        +--------+ User
%        |       /
%        |      /
%        |     /
%     dn |    /
%        |phi/
%        |  /
%        | /
%        |/
%        +
%        LH
%
% Input:
% @param[in] lh_n Lighthouse north component
% @return angle_rad Angle measurement in rad.
function [angle_rad] = lh_angle(lh_n, lh_e, user_n, user_e)
de = lh_e - user_e;
dn = lh_n - user_n;
angle_rad = atan2(de, dn)+pi;
end

% @brief Calculate the angles measured by the lighthouse
% system. Assumes the we can get all angles at the same epoch.
% Input:
% @param[in] lh_pos Position of the lighthouse basestations
% @param[in] user_pos True user position
% @param[in] sigma_rad White noise of the angular measurements
% @return angles Angle measurement for each lighthouse.
function [angles] = lh_angles(lh_pos, user_pos, sigma_rad)

lh_count = size(lh_pos,1);
angles = zeros(lh_count, 1);
for i=1:lh_count
    phi = lh_angle(lh_pos(i,1), lh_pos(i,2), user_pos(1), user_pos(2));
    angles(i) = phi + randn(1)*sigma_rad;
end

end

% @brief Cassini's Position Resection Method.
% Find coordinates of N, based on known positions A, B, C
% and the angles alpha (a) and beta (b).
%
%               B
%      A        |
%       \       |      C
%        \      |     /
%         \     |    /
%          \    |   /
%           \   |  /
%            \ a|b/
%             \ |/
%               N
%
% @param[in] an Point A north
% @param[in] ae Point A east
% @param[in] bn Point B north
% @param[in] be Point B east
% @param[in] cn Point C north
% @param[in] ce Point C east
% @param[in] alpha Angle between A and B in [rad]
% @param[in] beta Angle between B and C in [rad]
% @return Position point N [north, east]
function [pos] = cassini(an,ae,bn,be,cn,ce,alpha,beta)
th1y = ae - (an-bn)/tan(alpha);
th1x = an + (ae-be)/tan(alpha);
th2y = ce + (cn-bn)/tan(beta);
th2x = cn - (ce-be)/tan(beta);
t_h2_h1 = atan2(  th1y-th2y,  th1x-th2x);
t_b_n   = atan2(-(th1x-th2x), th1y-th2y);
t_a_n = t_b_n - alpha;
t_c_n = t_b_n + beta;
xP1 = th2x + ((be-th2y)-(bn-th2x)*tan(t_b_n))/(tan(t_h2_h1)-tan(t_b_n));
yP1 = th2y + (xP1-th2x)*tan(t_h2_h1);
xP2 = an  + ((ce-ae)-(cn-an)*tan(t_c_n))/(tan(t_a_n)-tan(t_c_n));
yP2 = ae  + (xP2-an)*tan(t_a_n);
pos = [0.5*(xP1+xP2); 0.5*(yP1+yP2)];
end

% @brief Position resection method with two base stations.
% Find coordinates of N, based on known positions A, B.
% and the angles phi_a and phi_b.
%
%        A
%         \         B
%          \       /
%           \     /
%            \   /
%             \ /
%              N
%
% @param[in] an Point A north
% @param[in] ae Point A east
% @param[in] bn Point B north
% @param[in] be Point B east
% @param[in] alpha Angle between A and B in [rad]
% @param[in] beta Angle between B and C in [rad]
% @return Position point N [north, east],
%         sol_found if a solution was found.
function [pos, sol_found] = twopointSolver(an,ae,bn,be,phi1,phi2)
twopoint_eps = 0.00001; % a bit of a random guess
v1 = [cos(phi1); sin(phi1)];
v2 = [cos(phi2); sin(phi2)];
l = [an - bn; ae - be];
M = [-v1(1), v2(1); -v1(2), v2(2)];
% Manual inversion of M (Cramer's rule)
invdetM = M(1,1)*M(2,2) - M(1,2)*M(2,1);
if (abs(invdetM) < twopoint_eps) % solution found?
    pos = [0;0];
    sol_found = false;
    return;
end
invM = 1/invdetM*[M(2,2), -M(1,2); -M(2,1), M(1,1)];
t = invM*l;
pos = [an;ae]+t(1)*v1;
pos2 = [bn;be]+t(2)*v2;
% check, are both solutions equal
sol_found = norm(pos-pos2) < twopoint_eps;
end

