T=0.2; %motor time constant (sec)
Kdc=11; %motor DC velocity gain (deg/sec)/(lego count)
m=0.5; %mass of the whole body + motors (kg)
R=0.042; %radius of the wheels (m)
L=0.1; %height of the c.g. above the wheel axel (m)
J=(m/12)*(0.15^2+0.04^2); %moment of inertia of the body (kg*m^2)
Jeff=(J+m*R*L+m*L*L); %effective total moment of inertia (kg*m^2)
g=9.8; % acceleration due to gravity (m/s^2)

%State space model:
A=[0 1 0 0;...
    m*g*L/Jeff 0 0 m*R*L/(T*Jeff); ...
    0 0 0 1; ...
    0 0 0 -1/T];
B=[0;-m*R*L*Kdc/(T*Jeff);0;Kdc/T];

C=[1 0 0 0;0 1 0 0;0 0 1 0];

D=[0;0;0];

%Pole placement:
%K=place(A,B,[-5+j -5-j -7+j -7-j]);

%Linear quadratic regulator:
  max_angle=10; %max body angle in deg
  max_rate=50; %max body rate in deg/sec
  max_position=1000; %max motor position in deg
  max_velocity=500; %max motor velocity in deg/sec
  max_drive=100; %lego drive units
% % 
  Q=diag([1/sqrt(max_angle) 1/sqrt(max_rate) 1/sqrt(max_position) 1/sqrt(max_velocity)]);
  R=1/sqrt(max_drive);
  N=zeros(4,1);
% % 
  [K,S,E]=lqr(A,B,Q,R,N)

%Kalman design 

G=B; %Process disturbance assumed to come in as an error in the motor drive
H=zeros(3,1); %Process disturbance does not directly affect outputs

SYS=ss(A,[B G],C,[D H]);

QN=50; %Process noise (motor drive error ... 3 units is deadzone ... so 3*3=9)
RN=diag([100;1;1]); %Measurement noise ... accel, then gyro then motor encoder, again this is expected value of noise squared

[KEST,L,P] = kalman(SYS,QN,RN);

K