A_4 = [0 1 0 0 0 0 
    0 0 0 0 0 0
    0 0 0 1 0 0
    0 0 0 0 0 0
    0 0 0 0 0 1
    K_3 0 0 0 0 0];

B_4 = [0 0
    0 K_1
    0 0
    K_2 0
    0 0
    0 0];

C_4 = diag([1 1 1 1 0 1]);

Ts = 0.002;

sysd = c2d(ss(A_4, B_4, C_4, 0), Ts);

%R_d on the ground:
%R_d_g = getfield(load('R_d-ground.mat'),'ans')';
%R_d = cov(R_d_g)

%R_d in the air:
R_d_a = getfield(load('R_d-air.mat'),'ans')';
R_d = cov(R_d_a);

Q_d = diag([0.001 0.001 0.001 0.001 0.001 0.001]);