% Clean up the environment
close all
clear all

% Constants
rad2deg = 180/pi;
deg2rad = pi/180;

% Set the time vector
start_time = 0;
stop_time = 10;
time_step = 0.01;
time_vec = start_time:time_step:stop_time;

% Force and torque values
on_force_val = 10;
on_torque_val = 3;

% Buid the force and torque vectors as functions of time
x_forcev = zeros(size(time_vec));
y_forcev = zeros(size(time_vec));
torquev = zeros(size(time_vec));
for i = 1:length(time_vec)
    if (time_vec(i) >= 7)
        x_forcev(i) = x_forcev(i) + on_force_val;
    end
    
    if (time_vec(i) <= 3)
        x_forcev(i) = x_forcev(i) - on_force_val;
    end
    
    if ((time_vec(i) >=2)&(time_vec(i) <= 8))
        torquev(i) = torquev(i) + on_torque_val;
    end
    
    if (time_vec(i) >= 6)
        torquev(i) = torquev(i) - on_torque_val;
    end
    
    if (time_vec(i) <= 4)
        y_forcev(i) = y_forcev(i) - on_force_val;
    end
    
    if (time_vec(i) >= 8)
        y_forcev(i) = y_forcev(i) + on_force_val;
    end
        
end

% Format the forces and torques for use with the source block
torque = [time_vec' torquev'];
x_force = [time_vec' x_forcev'];
y_force = [time_vec' y_forcev'];

% Process noise variances
% Note that these are squares of standard deviations, so they look pretty
% small
pos_proc_var = 0.01;
ang_proc_var = 0.001;

% Measurement noise variances
% Note that these are squares of standard deviations, so they look pretty
% small
pos_meas_var = 0.005;
ang_meas_var = 0.001;

% Run the dynamics from the Simulink model
% Check the simulation configuration dialog of the model to ensure that the
% following things are set:
%
% Start time:  start_time
% Stop time:  stop_time
% Type:  Fixed-step
% Solver:  ode3(Bogacki-Sharnpine)
% Periodic sample time constraint:  Unconstrained
% Fixed-step size (fundamental sample time):  time_step
% Tasking mode for periodic sample times:  Auto
% Higher priority value indicates higher task priority:  Unchecked
% Automatically handle data transfers between tasks:  Unchecked
%
% After running the simulation, the following variables are available:
%
% truth
% meas
%
% Both are structures that contain the data we're interested in
% The structure is automatically created by Simulink and is needlessly
% complicated ... They are extracted below for simplicity
sim('my_solution')

% Extract the truth and the measurements from the Simulink data structures
meas_vecs = meas.signals.values;
truth_vecs = truth.signals.values;

% Form the measurement covariance matrix
R = diag([pos_meas_var; pos_meas_var; ang_meas_var]);

% Form the continuous process noise covariance matrix
% Note that process noise is only added to the velocity states because
% we're really adding this to the state equation:
%
% X_dot = Phi*X + B*u + Q
Q = diag([0, 0, 0, pos_proc_var, pos_proc_var, ang_proc_var]);

% Put the input vectors into a matrix for easy use
input_vectors = [x_forcev', y_forcev', torquev'];

% Define the initial error variances on the estimate we start with
init_pos_err_var = 0.025;
init_vel_err_var = 1;
init_ang_err_var = 0.01;
init_ang_rate_err_var = 0.02;

% Initialize the state covariance estimate
P_init = diag([init_pos_err_var, init_pos_err_var, init_ang_err_var, init_vel_err_var, init_vel_err_var, init_ang_rate_err_var]);
P = P_init

% Initialize the state estimate
X_init = truth_vecs(1,:)' + sqrtm(P)*randn(6,1);
X = X_init;