To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 6563d5cc authored by roangel's avatar roangel
Browse files

Added LQR with clipping to demo controller for comparison

parent 0d2187dd
......@@ -48,7 +48,7 @@ shouldDisplayDebugInfo : false
# - Swaps between pitch set-points to test angle set-point response time
# i.e., this controller test the assumption that "the inner loop is infinitely fast"
#
controller_mode : 6
controller_mode : 5
# A flag for which estimator to use, defined as:
......@@ -61,7 +61,7 @@ controller_mode : 6
# each of (x,y,z,roll,pitch,yaw)
# 3 - Quad-rotor Model Based Method
# Uses the model of the quad-rotor and the previous inputs
estimator_method : 2
estimator_method : 1
# The LQR Controller parameters for "mode = 1"
......@@ -89,9 +89,10 @@ gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
# The LQR Controller parameters for "mode = 5"
gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
gainMatrixRollAngle_50Hz : [ 0.00,-1.1583, 0.00, 0.00,-0.6278, 0.00]
gainMatrixPitchAngle_50Hz : [ 1.1583, 0.00, 0.00, 0.6278, 0.00, 0.00]
gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.1255, 0.00, 0.00, 0.0912]
gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
......@@ -124,4 +125,4 @@ PMKF_Kinf_for_angles : [ 0.3046,11.0342]
#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
\ No newline at end of file
#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
......@@ -869,6 +869,18 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
}
float clipping_box_angle = 45*PI/180;
if(lqr_angleRateNested_prev_rollAngle > clipping_box_angle)
lqr_angleRateNested_prev_rollAngle = clipping_box_angle;
if(lqr_angleRateNested_prev_rollAngle < -clipping_box_angle)
lqr_angleRateNested_prev_rollAngle = -clipping_box_angle;
if(lqr_angleRateNested_prev_pitchAngle > clipping_box_angle)
lqr_angleRateNested_prev_pitchAngle = clipping_box_angle;
if(lqr_angleRateNested_prev_pitchAngle < -clipping_box_angle)
lqr_angleRateNested_prev_pitchAngle = -clipping_box_angle;
//ROS_INFO("Inner called");
// PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
......
......@@ -201,7 +201,7 @@ std::atomic<array<float,N_x>> shared_x0;
std::mutex MPC_mutex;
int N = 4;
int N = 7;
// ROS Publisher for debugging variables
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment