Skip to content

Latest commit

 

History

History
30 lines (22 loc) · 2.9 KB

README.md

File metadata and controls

30 lines (22 loc) · 2.9 KB

Double pendelum

The equations of motion are calculated using Lagrangian mechanics. The controller is calculated around the linearized point where all rods are pointing straight up and the angular velocities are zero. The controller is the optimal LQR controller. The control signal u is a torque applied to the base rod around the origin.

Equation of motion

\ \dot{\theta_1} = \omega_1
\ \dot{\theta_2} = \omega_2
\ \dot{\omega_1} = \frac{- 4 \omega_{2}^{2} \sin{\left(\theta_{1} - \theta_{2} \right)} - \left(2 \omega_{1}^{2} \sin{\left(\theta_{1} - \theta_{2} \right)} + 19.64 \sin{\left(\theta_{2} \right)}\right) \cos{\left(\theta_{1} - \theta_{2} \right)} + 19.64 \sin{\left(\theta_{1} \right)}}{2 \left(2 - \cos^{2}{\left(\theta_{1} - \theta_{2} \right)}\right)} \ \ \ \dot{\omega_2} = \frac{2 \omega_{1}^{2} \sin{\left(\theta_{1} - \theta_{2} \right)} + \omega_{2}^{2} \sin{\left(2 \theta_{1} - 2 \theta_{2} \right)} + 14.73 \sin{\left(\theta_{2} \right)} - 4.91 \sin{\left(2 \theta_{1} - \theta_{2} \right)}}{2 \left(2 - \cos^{2}{\left(\theta_{1} - \theta_{2} \right)}\right)} \

poles
The poles for the open loop system and closed loop system.

poles
Responce of the system with controller.

poles
Double pendelum without controller.

poles
Pendelum with controller.

Extended Kalman filter

ekf
Kalman filter to estimate the current state.