Matlab code for setting up Simulink Control. Observe only X and Theta

balancer.m

% Declare Constant Values

% Taken from Beaker2 (the robot)
mp=1.793;            % Mass of the pendulum (kg)
mw=0.156;            % Mass of the wheel (kg)
Len=0.209;           % Length to Center of Gravity(meters)
r=0.042;             % Radius of the wheel (meters)
ip=0.07832;          % inertia of the pendulum around the wheel axis
iw=0.000172125;      % inertia of the wheel around the wheel axis

% Taken from Pololu.com ( motor suppliers )
Rs=0.0024;           % The resistance of the DC Motor
ke=0.0342857;        % Voltage constant for the DC Motor
km=0.017;            % Motor Torque Constant of the DC Motor

% Taken from Nature
g=9.81;              % Gravity

% Declare additional constants to avoid repeated calculation
beta = 2*mw+((2*iw)/(r*r))+mp;
alpha = (ip*beta)+(2*mp*Len*Len*(mw+(iw/(r*r))));

% State-Space 'A' & 'B' Matrix Indices
a22=(2*km*ke*(mp*Len*r-ip-mp*Len*Len))/(Rs*r*r*alpha);
a23=(mp*mp*g*Len*Len)/alpha;
a42=(2*km*ke*(r*beta-mp*Len))/(Rs*r*r*alpha);
a43=(mp*g*Len*beta)/alpha;
b21=(2*km*(ip+mp*Len*Len-mp*Len*r))/(Rs*r*alpha);
b24=(2*km*(mp*Len-r*beta))/(Rs*r*alpha);

% Construct Continuous Time State-Space Matrices (x'=Ax+Bu , y=Cx+Du)
a=[0 1 0 0; 0 a22 a23 0; 0 0 0 1; 0 a42 a43 0];
b=[0; b21; 0; b24];
c=[1 0 0 0; 0 0 1 0]; % only observe x and theta
d=0;

% Name States, Inputs, and Outputs
states = {'xDot' 'xDDot' 'PhiDot' 'PhiDDot'};
inputs = ('Va');
outputs= {'x' 'Theta'};

% Construct continuous State-Space System
sys_cont = ss(a,b,c,d,'statename',states,'inputname',inputs,'outputname',outputs);

%% Discrete Time State-Space with LQR Modeling

% Set timestep value in seconds
Ts=0.01;

% Convert Continous Time Model to Discrete Time Model
sys_d = c2d(sys_cont,Ts,'zoh');

% Extract Discrete Time State-Space Matrices
A = sys_d.a;
B = sys_d.b;
C = sys_d.c;
D = sys_d.d;

% State Weights for Q weight matrix
w = 1;     % x (position) state variable weight
x = 1;     % xDot state variable weight
y = 10;    % theta (angle) state variable weight
z = 100;   % thetaDot state variable weight

% Construct Q matrix (symmetric diagonal)
Q = [w 0 0 0;
     0 x 0 0;
     0 0 y 0;
     0 0 0 z];

% Assign R value for LQR input weight
R = 1;

% Find LQR gain Matrix K
[K,S,e] = dlqr(A,B,Q,R);

%% Prepare and Create Kalman Filter

% Process Noise Measurement Covariance Matrix sometimes called `Q`
% Expect uniform measurement noise.
% v close w 0.0001
% even closer w 0.001
% 0.01 was good too.
% 1 was much worse

ProcessNoise = .01;

% Measurement Noise Covariance Matrix sometimes called 'R'.
% Expect very little noise with X, and a lot of noise with Theta.
% There should be little to no correlation between the two noises.
xMeasurementNoise = 0.000001;
thetaMeasurementNoise = 0.0001;
MeasurementNoise = diag([xMeasurementNoise thetaMeasurementNoise]);

% Use Kalman to estimate full system given only sensors of x and theta.
% Double up B because the filter has two inputs, X and Theta.
sys_kalman = ss(A, [B B], C, 0, Ts);
sensors = [1 2];
known = 1;
[kest,L,P] = kalman(sys_kalman,ProcessNoise,MeasurementNoise,[],sensors,known);

%% Form the LQG Regulator.
lqg_reg = lqgreg(kest, K);