A=[-0.8000   -0.0006  -13.2000         0
         0   -0.0140  -16.6400  -32.2000
    1.0000   -0.0001   -1.6500         0
    1.0000         0         0         0];
B=[-19.0000   -2.5000
   -0.6600   -0.5000
   -0.1600   -0.6000
         0         0];
C=[0     0     0     1
   0     0    -1     1];
Q=diag([.0365;.000025;8.2;.672]);
R=diag([3.6;3.6]);
Sw=0.01*eye(2);
Sv=Sw;
