Created LQR controller #210
Conversation
| state_matrix_t A_quadrotor(const state_vector_t& x, const control_vector_t& u); | ||
| control_gain_matrix_t B_quadrotor(const state_vector_t& x, const control_vector_t& u); | ||
|
|
||
| state_matrix_t A_quadrotor(const state_vector_t& x, const control_vector_t& u) |
There was a problem hiding this comment.
I don't think common is the right place for these matrix definitions. I think this should belong to the controller
There was a problem hiding this comment.
Agreed. However there is no LQR controller class, only a geometric controller class.
There was a problem hiding this comment.
@brunopinto900 I think we should make one 😄
| @@ -121,4 +132,171 @@ Eigen::Vector4d rot2Quaternion(const Eigen::Matrix3d &R) { | |||
| return quat; | |||
| } | |||
|
|
|||
| state_matrix_t A_quadrotor(const state_vector_t& x, const control_vector_t& u); | |||
| control_gain_matrix_t B_quadrotor(const state_vector_t& x, const control_vector_t& u); | |||
There was a problem hiding this comment.
This seems like an unnecessary declaration. All functions are inline anyway
| const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R, | ||
| Eigen::MatrixXd &P, const double dt, | ||
| const double &tolerance, | ||
| const uint iter_max) { |
There was a problem hiding this comment.
Why is this being calculated inline?
There was a problem hiding this comment.
This a function to calculate the ricatti equations, thats why its a header only function. I can put in the geometric controller class.
| @@ -50,6 +50,7 @@ geometricCtrl::geometricCtrl(const ros::NodeHandle &nh, const ros::NodeHandle &n | |||
| ctrl_enable_(true), | |||
| landing_commanded_(false), | |||
| feedthrough_enable_(false), | |||
| use_lqr_(false), | |||
There was a problem hiding this comment.
This means we can enable LQR and the geometric controller at the same time. Would it be possible to extend ctrl_mode_ so that we can choose controllers?
There was a problem hiding this comment.
When use_lqr is true, the geometric controller its not used. Although is much better to do it "via" ctrl_mode.
|
|
||
| typedef Eigen::Matrix<double, nStates, nStates> state_matrix_t; | ||
| typedef Eigen::Matrix<double, nControls, nControls> control_matrix_t; | ||
| typedef Eigen::Matrix<double, nStates, nControls> control_gain_matrix_t; |
There was a problem hiding this comment.
I don't think common is the right place for these typedefs. This should belong to the controller since these are not shared with any other controller
There was a problem hiding this comment.
Makes sense. I can declare on the geometric controller class.
|
@brunopinto900 Are there any updates on this? Or can I take this over and move this forward? |
|
Hello. Please do. No update. I started a job and with school i won't have time. |
|
@brunopinto900 Thanks, closing for now |
Hello,
This PR implements a LQR controller based on the paper Foehn, Philipp & Scaramuzza, Davide. (2018). Onboard State Dependent LQR for Agile Quadrotors. 10.1109/ICRA.2018.8460885.
Thank you.