A = zeros(6,6);
A(1,1) = -2.82e-8;
A(2,2) =-6.76e-3;
A(3:4,3:4) = [-0.122 1.57; -1.57 -0.122];
A(5:6,5:6) = [-0.71 21.3; -21.3 -0.71];
Bu = [-4.06e-4 -2.2e-1 8.84e-2 -3.08e-1 6.39e-2 -1.08]';
C = [-3.5e-4 -1.57e-1 -1.56e-1 4.61e-3 -3.37e-1 -2.40e-2;
    -2.05e-4 -1.54e-1 2.81e-1 -9.85e-2 -1.02 -7.68e-2];
Bw = [-1.65e-7 7.61e-5 3.05e-2 -1.05e-2 4.46e-3 -1.02e-2]';
Sw = 1;
Sv = eye(2)*1e-6;
