clear;
clc;
% Declare symbolic variables
syms x1 x2 x3 x4 Mc Jc u x_10 J g Rc
f= [x3; x4; (-Mc*g*sin(x2)+Mc*x1*x4^2)/(Jc/Rc^2+Mc);
(-2*Mc*x1*x3*x4-Mc*g*x1*cos(x2)+u)/(Mc*x1^2+J+Jc)];
h = x1;
%Compute Jacobian
dfdx = jacobian(f, [x1 x2 x3 x4]);
dfdu = jacobian (f, u);
dhdx = jacobian(h, [x1 x2 x3 x4]);
dhdu = jacobian(h, u);
%Compute A B C and D
A = subs( dfdx, [x1 x2 x3 x4 u], [x_10 0 0 0 Mc*g*x_10] )
B = subs( dfdu, [x1 x2 x3 x4 u], [x_10 0 0 0 Mc*g*x_10] )
C = subs( dhdx, [x1 x2 x3 x4 u], [x_10 0 0 0 Mc*g*x_10] )
D= subs( dhdu, [x1 x2 x3 x4 u], [x_10 0 0 0 Mc*g*x_10] )
A =
[0, 0, 1, 0]
[0, 0, 0, 1]
[0, -(Mc*g)/(Mc + Jc/Rc^2), 0, 0]
[ -(Mc*g)/(Mc*x_10^2 + J + Jc),0, 0, 0]
B =
0
0
0
1/(Mc*x_10^2 + J + Jc)
C =
[ 1, 0, 0, 0]
D =
0