This is my inverted pendulum
And here is the all equations:
Potential energy: $$V = \frac{g l_{1}}{2} m_{1} \cos{\left (\theta{\left (t \right )} \right )} + g l_{2} m_{2} \cos{\left (\theta{\left (t \right )} \right )}$$
Kinetic energy: $$T = \frac{M}{2} \left(\frac{d}{d t} x{\left (t \right )}\right)^{2} + \frac{l_{1}^{2} m_{1}}{2} \sin^{2}{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + \frac{l_{2}^{2} m_{2}}{2} \sin^{2}{\left (\theta{\left (t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + \frac{m_{1}}{2} \left(l_{1} \cos{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} + \frac{d}{d t} x{\left (t \right )}\right)^{2} + \frac{m_{2}}{2} \left(l_{2} \cos{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} + \frac{d}{d t} x{\left (t \right )}\right)^{2}$$
And the langarian difference:
$$L = \frac{M}{2} \left(\frac{d}{d t} x{\left (t \right )}\right)^{2} - \frac{g l_{1}}{2} m_{1} \cos{\left (\theta{\left (t \right )} \right )} - g l_{1} m_{2} \cos{\left (\theta{\left (t \right )} \right )} + \frac{l_{1}^{2} m_{1}}{2} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + l_{1} m_{1} \cos{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} \frac{d}{d t} x{\left (t \right )} + \frac{l_{2}^{2} m_{2}}{2} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + l_{2} m_{2} \cos{\left (\theta{\left (t \right )} \right )} \frac{d}{d t} \theta{\left (t \right )} \frac{d}{d t} x{\left (t \right )} + \frac{m_{1}}{2} \left(\frac{d}{d t} x{\left (t \right )}\right)^{2} + \frac{m_{2}}{2} \left(\frac{d}{d t} x{\left (t \right )}\right)^{2}$$
And now the langarian equation:
$$\frac{d}{dt}(\frac{\partial L}{\partial \dot{q_1}}) - \frac{\partial L}{\partial {q_1}} = Q_i$$
And that will be:
$$M \frac{d^{2}}{d t^{2}} x{\left (t \right )} - l_{1} m_{1} \sin{\left (\theta{\left (t \right )} \right)} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + l_{1} m_{1} \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} - l_{2} m_{2} \sin{\left (\theta{\left(t \right )} \right )} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} + l_{2} m_{2} \cos{\left(\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + m_{1} \frac{d^{2}}{d t^{2}} x{\left (t \right )} + m_{2} \frac{d^{2}}{d t^{2}} x{\left (t \right )} = F - Kx - B\dot{x}$$
$$- \frac{g l_{1}}{2} m_{1} \sin{\left (\theta{\left (t \right )} \right )} - g l_{2} m_{2} \sin{\left (\theta{\left (t \right )} \right )} + l_{1}^{2} m_{1} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + l_{1} m_{1} \cos{\left (\theta{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}} x{\left (t \right )} + l_{2}^{2} m_{2} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + l_{2} m_{2} \cos{\left (\theta{\left (t\right )} \right )} \frac{d^{2}}{d t^{2}} x{\left (t \right )} = -k \theta - b \dot{\theta}$$
Because the force inputs: $$Q_i = \begin{bmatrix} F - Kx - B\dot{x}\\ -k \theta - b \dot{\theta} \end{bmatrix}$$
Octave code:
pkg load symbolic
% Symbols
syms m2 g l1 theta(t) m1 l2 M x(t)
% Potential energy
V = m2*g*l2*cos(theta) + m1*g*l1*1/2*cos(theta);
% Kinect energy
T = 1/2*M*diff(x)^2 + 1/2*m2*(diff(x) + l2*diff(theta)*cos(theta))^2 + 1/2*m2*( - l2*diff(theta)*sin(theta))^2 + 1/2*m1*(diff(x)+l1*diff(theta)*cos(theta))^2 + 1/2*m1*( - l1*diff(theta)*sin(theta))^2;
% Difference
L = simplify(T - V);
% Lagrange equation
dL = simplify(jacobian(L, [x theta]));
ddL = simplify(diff(jacobian(L, [diff(x) diff(theta)])));
% Difference
A = simplify(ddL - dL);
% Show each row
simplify(A(1))
simplify(A(2))
% Linearize sin theta(t) = theta, cos theta(t) = 1
B = simplify(subs(A, [sin(theta(t)) cos(theta(t)) diff(theta, 1)^2], [theta(t) 1 0]));
B(1)
B(2)
And if I want to linearize those equation when the rod is pointing straight up, I can just say that $$sin (\theta) = \theta \\ cos (\theta) = 1 \\ \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2} = 0, \text{then} \space sin (\theta) = 0$$
And now the equations are linear.
$$M \frac{d^{2}}{d t^{2}} x{\left (t \right )} + l_{1} m_{1} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + l_{2} m_{2} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + m_{1} \frac{d^{2}}{d t^{2}} x{\left (t \right )} +m_{2} \frac{d^{2}}{d t^{2}} x{\left (t \right )} = F - Kx - B\dot{x}$$
$$- \frac{g l_{1}}{2} m_{1} \theta{\left (t \right )} - g l_{2} m_{2} \theta{\left (t \right )} + l_{1}^{2} m_{1} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + l_{1} m_{1} \frac{d^{2}}{d t^{2}} x{\left (t \right )} + l_{2}^{2} m_{2} \frac{d^{2}}{d t^{2}} \theta{\left (t \right )} + l_{2} m_{2} \frac{d^{2}}{d t^{2}} x{\left (t \right )} = -k \theta - b \dot{\theta}$$ Question:
How can I build a state space model if I got two different second derivative in the same equation? In this case it's $\ddot{x}$ and $\ddot{\theta}$
Edit:
Thanks Kwin!
Edit2:
It works now. Here is a test
M = 10;
m1 = 2;
m2 = 5;
l1 = 5;
l2 = 5.4;
K = 3;
B = 2;
g = 9.82;
k = 4;
b = 2;
M = [(M+m1+m2) (l1*m1 + m2*l2); (l1*m1 + m2*l2) (l1^2*m1 +l2^2*m2)];
A = [-K -B 0 0; 0 0 (1/2*g*l1*m1 + g*l2*m2 -k) -b ];
Omega = inv(M)*E;
A = [0 1 0 0; Omega(1,1) Omega(1,2) Omega(1,3) Omega(1,4); 0 0 0 1; Omega(2,1) Omega(2,2) Omega(2,3) Omega(2,4)];
B = [0; 1; 0; 0];
sys = ss(0, A, B);
pole(sys)
ans =
-1.65310 + 0.00000i
-0.06384 + 0.42388i
-0.06384 - 0.42388i
1.56360 + 0.00000i
Q = eye(4);
R = 1;
L = lqr(sys, Q, R);
regsys = reg(sys, L);
pole(regsys)
ans =
-0.84316 + 0.59903i
-0.84316 - 0.59903i
-1.65264 + 0.00000i
-1.54574 + 0.00000i
Remember that the positive eigenvalue from pole(sys) is probably the rotation $\theta$ of the cart. Giving the pendulum a negative feedback, then the feedback model would have all negative eigenvalues, which means stable system.
If you have a differential equation of the second order
$$ \ddot{x} = f(t, x, \dot{x}) $$
then you can always write it as first order differential equation as well by defining a new state vector as $z = \begin{bmatrix}x^\top & \dot{x}^\top\end{bmatrix}^\top$ such that
$$ \dot{z} = \begin{bmatrix}\dot{x} \\ f(t,x,\dot{x})\end{bmatrix}. $$
In your case your differential equation can be written as
$$ \mathbf{M} \begin{bmatrix} \ddot{x} \\ \ddot{\theta} \end{bmatrix} = f(t, x, \theta, \dot{x}, \dot{\theta}) $$
with
$$ \mathbf{M} = \begin{bmatrix} M + m_1 + m_2 & l_1\,m_1 + l_2\,m_2 \\ l_1\,m_1 + l_2\,m_2 & l_1^2\,m_1 + l_2^2\,m_2 \end{bmatrix}. $$
Assuming that $\mathbf{M}$ is invertible, then your differential equation can also be written as
$$ \begin{bmatrix} \ddot{x} \\ \ddot{\theta} \end{bmatrix} = \mathbf{M}^{-1}\,f(t, x, \theta, \dot{x}, \dot{\theta}) $$
which is of the same form as at the start of this answer, only now you have to define the new state space as $z = \begin{bmatrix}x & \theta & \dot{x} & \dot{\theta}\end{bmatrix}^\top$.