close all

% ## General parameters
% Fix random seed so everyone gets the same true trajectory.
rng(51)
% Sampling time:
dt = 0.2;
% System noise:
R = diag([0.02^2, 0.02^2, 0.02^2]);
% Measurement noise (1D measurements):
Q = eye(4)*1.0^2;
% Construct vector with control input:
uhc = repmat([0.1; 0.105], 1, 100);
uhc2 = repmat([0.105; 0.1], 1, 100);
ust = repmat([0.1; 0.1], 1, 30);
u = [uhc, ust, uhc2, ust];

% ## Compute the true trajectory and measurements:
%    Evaluate kinematic model according to control input
%    and add some sampled system noise.
currx = [0; 0; 0];
x_true = currx;
z_meas = [];
for i = 1:size(u,2)
    % Evaluate kinematic Model:
    currx = motion_diff(currx, u(:,i));
    % Add system noise:
    currx = currx + mvnrnd([0; 0; 0], R)';
    % Simulate a measurement:
    z_meas = [z_meas, measurement(currx) + mvnrnd(zeros(size(Q,1),1),Q)'];
    % Store result for plotting
    x_true = [x_true, currx];
end

% Reinitialize random seed generator for random sampling
rng('shuffle')