param_plant.m

% NXT Ballbot Parameters and State-Space Matrix Calculation

% Physical Constant
g = 9.81;					% gravity acceleration [m/sec^2]

% NXT Ballbot Parameters
Ms = 0.013;					% spherical wheel (ball) weight [kg]
Rs = 0.026;					% spherical wheel (ball) radius [m]
Js = 2 * Ms * Rs^2 / 3;		% spherical wheel (ball) inertia moment [kgm^2]
Mw = 0.015;					% motor wheel weight [kg]		
Rw = 0.021;					% motor wheel radius [m]
Lw = 0.022;					% motor wheel height [m]
Jw = Mw * Lw^2 / 2;			% motor wheel inertia moment [kgm^2]
Mb = 0.682;					% body weight (including wheel weight) [kg]
L = 0.17;					% distance between the center of mass and the center of spherical wheel [m]
Jpsi = Mb * L^2 / 3;		% body pitch inertia moment [kgm^2]
fm = 0.0022;				% friction coefficient between body and DC motor
fs = 0;						% friction coefficient between floor and spherical wheel (ball)

% DC Motor Parameters			
Jm = 1e-5;					% DC motor inertia moment [kgm^2]
Rm = 6.69;					% DC motor resistance []
Kb = 0.468;					% DC motor back EMF constant [Vsec/rad]
Kt = 0.317;					% DC motor torque constant [Nm/A]

% NXT Ballbot State-Space Matrix Calculation
k = Rs / Rw;
alpha = Kt / Rm * 180 / pi;
beta = k * (Kt * Kb / Rm + fm);
tmp = beta + fs;

E_11 = (Mb + Ms) * Rs^2 + Js + k^2 * (Jm + Jw);
E_12 = Mb * L * Rs - k^2 * (Jm + Jw);
E_22 = Mb * L^2 + Jpsi + k^2 * (Jm + Jw);
detE = E_11 * E_22 - E_12^2;

A_32 = -g * Mb * L * E_12 / detE;
A_42 = g * Mb * L * E_11 / detE;
A_33 = -(tmp * E_22 + beta * E_12) / detE;
A_43 = (tmp * E_12 + beta * E_11) / detE;
A_34 = beta * (E_22 + E_12) / detE;
A_44 = -beta * (E_11 + E_12) / detE;
B_3 = alpha * (E_22 + E_12) / detE;
B_4 = -alpha * (E_11 + E_12) / detE;
A = [
	0 0 1 0
	0 0 0 1
	0 A_32 A_33 A_34
	0 A_42 A_43 A_44
	];
B = [
	0
	0
	B_3
	B_4
	];
C = eye(4);
D = zeros(4, 1);

clear k alpha beta tmp
clear E_11 E_12 E_22 detE
clear A_32 A_33 A_34 A_42 A_43 A_44 B_3 B_4