AC_CustomControl: add LQR controller

libraries/AC_CustomControl/AC_CustomControl.cpp

@@ -7,6 +7,7 @@
 #include "AC_CustomControl_Backend.h"
 // #include "AC_CustomControl_Empty.h"
 #include "AC_CustomControl_PID.h"
+#include "AC_CustomControl_LQR.h"
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
 
@@ -15,7 +16,7 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Custom control type
     // @Description: Custom control type to be used
-    // @Values: 0:None, 1:Empty, 2:PID
+    // @Values: 0:None, 1:Empty, 2:PID, 3:LQR-integral
     // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO_FLAGS("_TYPE", 1, AC_CustomControl, _controller_type, 0, AP_PARAM_FLAG_ENABLE),
@@ -33,6 +34,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = {
     // parameters for PID controller
     AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]),
 
+    // parameters for LQR controller
+    AP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]),
+
     AP_GROUPEND
 };
 
@@ -62,6 +66,10 @@ void AC_CustomControl::init(void)
             _backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt);
             _backend_var_info[get_type()] = AC_CustomControl_PID::var_info;
             break;
+        case CustomControlType::CONT_LQR:
+            _backend = new AC_CustomControl_LQR(*this, _ahrs, _att_control, _motors, _dt);
+            _backend_var_info[get_type()] = AC_CustomControl_LQR::var_info;
+            break;
         default:
             return;
     }

libraries/AC_CustomControl/AC_CustomControl.h

@@ -14,7 +14,7 @@
 #include <AP_Motors/AP_MotorsMulticopter.h>
 
 #ifndef CUSTOMCONTROL_MAX_TYPES
-#define CUSTOMCONTROL_MAX_TYPES 2
+#define CUSTOMCONTROL_MAX_TYPES 3
 #endif
 
 class AC_CustomControl_Backend;
@@ -49,6 +49,7 @@ class AC_CustomControl {
         CONT_NONE            = 0,
         CONT_EMPTY           = 1,
         CONT_PID             = 2,
+        CONT_LQR             = 3,
     };            // controller that should be used     
 
     enum class  CustomControlOption {

libraries/AC_CustomControl/AC_CustomControl_LQR.cpp

@@ -0,0 +1,114 @@
+#include "AC_CustomControl_LQR.h"
+
+#if CUSTOMCONTROL_LQR_ENABLED
+
+#include <GCS_MAVLink/GCS.h>
+
+// table of user settable parameters
+const AP_Param::GroupInfo AC_CustomControl_LQR::var_info[] = {
+    // @Param: PARAM1
+    // @DisplayName: LQR param1
+    // @Description: Dumy parameter for LQR custom controller backend
+    // @User: Advanced
+    AP_GROUPINFO("PARAM1", 1, AC_CustomControl_LQR, param1, 0.0f),
+
+    // @Param: PARAM2
+    // @DisplayName: LQR param2
+    // @Description: Dumy parameter for LQR custom controller backend
+    // @User: Advanced
+    AP_GROUPINFO("PARAM2", 2, AC_CustomControl_LQR, param2, 0.0f),
+
+    // @Param: PARAM3
+    // @DisplayName: LQR param3
+    // @Description: Dumy parameter for LQR custom controller backend
+    // @User: Advanced
+    AP_GROUPINFO("PARAM3", 3, AC_CustomControl_LQR, param3, 0.0f),
+
+    AP_GROUPEND
+};
+
+// initialize in the constructor
+AC_CustomControl_LQR::AC_CustomControl_LQR(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) :
+    AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt)
+{
+    _dt = dt;
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// update controller
+// return roll, pitch, yaw controller output
+Vector3f AC_CustomControl_LQR::update(void)
+{
+    // reset controller based on spool state
+    switch (_motors->get_spool_state()) {
+        case AP_Motors::SpoolState::SHUT_DOWN:
+        case AP_Motors::SpoolState::GROUND_IDLE:
+            // We are still at the ground. Reset custom controller to avoid
+            // build up, ex: integrator
+            reset();
+            break;
+
+        case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
+        case AP_Motors::SpoolState::SPOOLING_UP:
+        case AP_Motors::SpoolState::SPOOLING_DOWN:
+            // we are off the ground
+            break;
+    }
+
+    // arducopter main attitude controller already runned
+    // we don't need to do anything else
+
+    //gcs().send_text(MAV_SEVERITY_INFO, "LQR custom controller working");
+
+    Quaternion attitude_body, attitude_target;
+   _ahrs->get_quat_body_to_ned(attitude_body);
+
+
+    attitude_target = _att_control->get_attitude_target_quat();
+
+
+    Quaternion attitude_error_quat = attitude_target.inverse() * attitude_body;
+
+
+// axis-angle representation of quaternion is used. The resulting //state-space for attitude control is [v1 v2 v3 p q r]
+    Vector3f attitude_error_angle;
+    attitude_error_quat.to_axis_angle(attitude_error_angle);
+
+
+    Vector3f gyro_latest = _ahrs->get_gyro_latest();
+
+
+// k_gain LQR matrix is defined as [kij], where i:[T1 T2 T3](torques in  // three axis) and j:[v1 v2 v3 p q r](roll pitch yaw roll-rate //pitch-rate yaw-rate)
+// k_gain = [k11 k12 k13 k14 k15 k16;
+//       k21 k22 k23 k24 k25 k26;
+//       k31 k32 k33 k34 k35 k36;]
+// [T1 T2 T3] = -k_gain*[v1 v2 v3 p q r]
+    _integralX += 0.03*attitude_error_angle.x*_dt;
+    _integralY += 0.08*attitude_error_angle.y*_dt;
+    _integralZ += 0.005*attitude_error_angle.z*_dt;
+    constrain_float(_integralX, -_kimax_LQR, _kimax_LQR);
+    constrain_float(_integralY, -_kimax_LQR, _kimax_LQR);
+    constrain_float(_integralZ, -_kimax_LQR, _kimax_LQR);
+
+    Vector3f motor_out;
+
+    motor_out.x = -_integralX - 0.375*attitude_error_angle.x - 0.09*gyro_latest.x;
+
+    motor_out.y = -_integralY - 0.375*attitude_error_angle.y - 0.125*gyro_latest.y;
+
+    motor_out.z = -_integralZ - 0.40*attitude_error_angle.z - 0.09*gyro_latest.z;
+
+   return motor_out;
+
+}
+
+// reset controller to avoid build up on the ground
+// or to provide bumpless transfer from arducopter main controller
+void AC_CustomControl_LQR::reset(void)
+{
+    _integralX = 0;
+    _integralY = 0;
+    _integralZ = 0;
+}
+
+#endif

libraries/AC_CustomControl/AC_CustomControl_LQR.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "AC_CustomControl_Backend.h"
+
+#ifndef CUSTOMCONTROL_LQR_ENABLED
+    #define CUSTOMCONTROL_LQR_ENABLED AP_CUSTOMCONTROL_ENABLED
+#endif
+
+#if CUSTOMCONTROL_LQR_ENABLED
+
+class AC_CustomControl_LQR : public AC_CustomControl_Backend {
+public:
+    AC_CustomControl_LQR(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt);
+
+
+    Vector3f update(void) override;
+    void reset(void) override;
+    // user settable parameters
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+    // declare parameters here
+
+    float _dt;
+
+    float _integralX, _integralY, _integralZ;
+    float _kimax_LQR = 0.1;
+
+    AP_Float param1;
+    AP_Float param2;
+    AP_Float param3;
+};
+
+#endif

libraries/AC_CustomControl/README.md

@@ -243,9 +243,9 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V
     _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
     _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
     _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
-    _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt),
-    _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, dt),
-    _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt)
+    _pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt),
+    _pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt),
+    _pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -256,3 +256,7 @@ AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl &frontend, AP_AHRS_V
 12. Add reset functionality inside `reset` function of `AC_CustomControl_XYZ.cpp` file. It is user's responsibility to add proper controller resetting functionality. This highly depends on the controller and it should not be copy-pasted from another backend without testing it in SITL.
 
 13. You can access target attitude and target rate values using `atti_control->get_attitude_target_quat()` and `atti_control->rate_bf_targets()` functions, respectively. You can also access latest gyro measurement using `_ahrs->get_gyro_latest()` function. Take a look at other backends
+
+## Integral action Linear Quadratic Regulator controller
+
+1. This controller serves as an example on using this library for deploying your own attitude controllers. The LQR controller implemented here essentially works like an attitude PID controller, wherein the PID gains for each axis have been tuned by solving the Algebraic Riccati Equation (ARE) for the attitude state-space of a quadcopter. Details about quadcopter dynamics are documented here: https://ardupilot.org/copter/docs/systemid-mode-operation.html#identification-of-a-multicopter. For further details behind the mathematics and implementation of this attitude controller, refer to this publication: https://www.researchgate.net/publication/279448184_Quadrotor_Quaternion_Control and this article: https://www.mathworks.com/help/control/ref/ss.lqi.html