r = 1;
m = 10;
I = [1 0 0 ; 0 1 0 ; 0 0 1]*.01;

seat_cg = [0 -0.3 0];
seat_m = 40;
seat_I = [1 0 0 ; 0 1 0 ; 0 0 1]*.1;

w = .7;
h = .3;

p1 = [-w h 0];
p2 = [w h 0];
p3 = [-w -h 0];
p4 = [w -h 0];

s1 = [-1 0 0];
s2 = [-1 -r 0];
s3 = [12 0 0];
s4 = [12 -r 0];
tau = 0.01;

mdl = 'Diwheel';
% Set the controller to zero during linearization
K = [0 0 0 0];
% Construct linearization input/output
io(1) = linio([ mdl '/Ctrl'],1,'in','on')
io(2) = linio([ mdl '/Sensors'],1,'out','on')
% Linearization
sys = linearize(mdl,io);


% Get minimal realization
[sys U] = minreal(sys);
% Design state feedback controller
Q = diag([ 100 1 100 1],0);
R = 1;
[K,S,e] = lqr(sys,Q,R);
% Transformation from y to x
K = K/sys.C;