diff --git a/pps_ws/src/d_fall_pps/param/MpcController.yaml b/pps_ws/src/d_fall_pps/param/MpcController.yaml
index 8a133950ad1a378b9345b7b5d832416f51e33443..8288be1dafa339d67b42ebc30ec65174d028b37b 100644
--- a/pps_ws/src/d_fall_pps/param/MpcController.yaml
+++ b/pps_ws/src/d_fall_pps/param/MpcController.yaml
@@ -5,4 +5,20 @@ mass : 27
control_frequency : 200
# Quadratic motor regression equation (a0, a1, a2)
-motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
\ No newline at end of file
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
diff --git a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
index 3b06b35196761b33807fbd4e42f5029a9b7fc1ae..48f24906d5ed309080481604871c8a6060adf231 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -94,9 +94,21 @@
// command directly to each of the motors, and additionally request the
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
-#define MOTOR_MODE 6
-#define RATE_MODE 7
-#define ANGLE_MODE 8
+
+#define CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+int estimator_method = ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+float estimator_frequency = 200;
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
@@ -114,9 +126,6 @@
using namespace d_fall_pps;
-
-
-
// ----------------------------------------------------------------------------------
// V V A RRRR III A BBBB L EEEEE SSSS
// V V A A R R I A A B B L E S
@@ -125,6 +134,25 @@ using namespace d_fall_pps;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+float current_xzy_rpy_measurement[6];
+float previous_xzy_rpy_measurement[6];
+
+
// Variables for controller
float cf_mass; // Crazyflie mass in grams
std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
@@ -136,13 +164,6 @@ float previous_stateErrorInertial[9]; // The location error of the Crazyflie
std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
-// The LQR Controller parameters for "LQR_RATE_MODE"
-const float gainMatrixRollRate[9] = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
-const float gainMatrixPitchRate[9] = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
-const float gainMatrixYawRate[9] = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
-const float gainMatrixThrust[9] = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
-
-
// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
@@ -198,9 +219,6 @@ int my_agentID = 0;
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
-// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
-void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
-
// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);
@@ -220,6 +238,10 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
+void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response);
+void perform_estimator_update_state_interial(Controller::Request &request, float (&stateInertialEstimate)[12]);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12]);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12]);
@@ -344,319 +366,229 @@ void processFetchedParameters();
//
//
// This function WILL NEED TO BE edited for successful completion of the PPS exercise
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
-{
-
- // This is the "start" of the outer loop controller, add all your control
- // computation here, or you may find it convienient to create functions
- // to keep you code cleaner
-
- // Define a local array to fill in with the state error
- float stateErrorInertial[9];
-
- // Fill in the (x,y,z) position error
- stateErrorInertial[0] = request.ownCrazyflie.x - setpoint[0];
- stateErrorInertial[1] = request.ownCrazyflie.y - setpoint[1];
- stateErrorInertial[2] = request.ownCrazyflie.z - setpoint[2];
-
- // Compute an estimate of the velocity
- // > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - previous_stateErrorInertial[0]) * control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - previous_stateErrorInertial[1]) * control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - previous_stateErrorInertial[2]) * control_frequency;
-
- // Fill in the roll and pitch angle measurements directly
- stateErrorInertial[6] = request.ownCrazyflie.roll;
- stateErrorInertial[7] = request.ownCrazyflie.pitch;
-
- // Fill in the yaw angle error
- // > This error should be "unwrapped" to be in the range
- // ( -pi , pi )
- // > First, get the yaw error into a local variable
- float yawError = request.ownCrazyflie.yaw - setpoint[3];
- // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
- while(yawError > PI) {yawError -= 2 * PI;}
- while(yawError < -PI) {yawError += 2 * PI;}
- // > Third, put the "yawError" into the "stateError" variable
- stateErrorInertial[8] = yawError;
-
-
- // CONVERSION INTO BODY FRAME
- // Conver the state erorr from the Inertial frame into the Body frame
- // > Note: the function "convertIntoBodyFrame" is implemented in this file
- // and by default does not perform any conversion. The equations to convert
- // the state error into the body frame should be implemented in that function
- // for successful completion of the PPS exercise
- float stateErrorBody[9];
- convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
-
-
- // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
- // > as we have already used previous error we can now update it update it
- for(int i = 0; i < 9; ++i)
- {
- previous_stateErrorInertial[i] = stateErrorInertial[i];
- }
-
-
- // **********************
- // Y Y A W W
- // Y Y A A W W
- // Y A A W W
- // Y AAAAA W W W
- // Y A A W W
- //
- // YAW CONTROLLER
-
- // Instantiate the local variable for the yaw rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float yawRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the yaw rate to respoond with
- for(int i = 0; i < 9; ++i)
+void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response)
+{
+ float roll_sp = input_angle[0];
+ float pitch_sp = input_angle[1];
+ float yaw_sp = input_angle[2];
+ float ft_sp = input_angle[3];
+
+ // initialize variables that will contain w_x, w_y and w_z
+ float w_x_sp = 0;
+ float w_y_sp = 0;
+ float w_z_sp = 0;
+
+ // Create the angle error to use for the inner controller
+ float angle_error[3] = {
+ stateInertial[6] - roll_sp,
+ stateInertial[7] - pitch_sp,
+ stateInertial[8] - yaw_sp
+ };
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 3; ++i)
{
- yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ // BODY FRAME Y CONTROLLER
+ w_x_sp -= gainMatrixRollRate_Nested[i] * angle_error[i];
+ // BODY FRAME X CONTROLLER
+ w_y_sp -= gainMatrixPitchRate_Nested[i] * angle_error[i];
+ // BODY FRAME Z CONTROLLER
+ w_z_sp -= gainMatrixYawRate_Nested[i] * angle_error[i];
}
- // Put the computed yaw rate into the "response" variable
- response.controlOutput.yaw = yawRate_forResponse;
-
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
-
- // **************************************
- // BBBB OOO DDDD Y Y ZZZZZ
- // B B O O D D Y Y Z
- // BBBB O O D D Y Z
- // B B O O D D Y Z
- // BBBB OOO DDDD Y ZZZZZ
- //
- // ALITUDE CONTROLLER (i.e., z-controller)
-
- // Instantiate the local variable for the thrust adjustment that will be
- // requested from the Crazyflie's on-baord "inner-loop" controller
- float thrustAdjustment = 0;
-
- // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
- for(int i = 0; i < 9; ++i)
- {
- thrustAdjustment -= gainMatrixThrust[i] * stateErrorBody[i];
- }
-
- // Put the computed thrust adjustment into the "response" variable,
- // as well as adding the feed-forward thrust to counter-act gravity.
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = w_x_sp;
+ response.controlOutput.pitch = w_y_sp;
+ response.controlOutput.yaw = w_z_sp;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
- // > NOTE: the "gravity_force" value was already divided by 4 when is was
- // loaded from the .yaml file via the "fetchYamlParameters"
- // class function in this file.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
-
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y X X
- // B B O O D D Y Y X X
- // BBBB O O D D Y X
- // B B O O D D Y X X
- // BBBB OOO DDDD Y X X
- //
- // BODY FRAME X CONTROLLER
-
- // Instantiate the local variable for the pitch rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float pitchRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the pitch rate to respoond with
- for(int i = 0; i < 9; ++i)
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(ft_sp/4.0);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(ft_sp/4.0);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(ft_sp/4.0);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(ft_sp/4.0);
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
{
- pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ ROS_INFO_STREAM("thrust =" << ft_sp);
+ ROS_INFO_STREAM("rollrate =" << w_x_sp);
+ ROS_INFO_STREAM("pitchrate =" << w_y_sp);
+ ROS_INFO_STREAM("yawrate =" << w_z_sp);
}
+}
- // Put the computed pitch rate into the "response" variable
- response.controlOutput.pitch = pitchRate_forResponse;
-
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y Y Y
- // B B O O D D Y Y Y Y
- // BBBB O O D D Y Y
- // B B O O D D Y Y
- // BBBB OOO DDDD Y Y
- //
- // BODY FRAME Y CONTROLLER
-
- // Instantiate the local variable for the roll rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float rollRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the roll rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
- }
- // Put the computed roll rate into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+ // This is the "start" of the outer loop controller, add all your control
+ // computation here, or you may find it convienient to create functions
+ // to keep you code cleaner
+ // Define a local array to fill in with the state error
+ float stateInertialEstimate[12];
+ perform_estimator_update_state_interial(request, stateInertialEstimate);
- // PREPARE AND RETURN THE VARIABLE "response"
+ // The estimate of the state is inside stateInertialEstimate now. Next, compute the error
+ // Should we convert into body frame for our MPC controller? Let's skip it for now, YAW is going to be zero in our case
- /*choosing the Crazyflie onBoard controller type.
- it can either be Motor, Rate or Angle based */
- // response.controlOutput.onboardControllerType = MOTOR_MODE;
- response.controlOutput.onboardControllerType = RATE_MODE;
- // response.controlOutput.onboardControllerType = ANGLE_MODE;
+ float x0_full_state[12] = {-setpoint[0], -setpoint[1], -setpoint[2], 0, 0, 0, 0, 0, 0, 0, 0, 0};
+ float correction_z = -0.82*(0.4 - stateInertialEstimate[2]) - 0.22 * (0 - stateInertialEstimate[5]) + cf_mass/1000*9.8;
+ float input_angle[4] = {-setpoint[0], 0, 0, correction_z};
+ // create a function that takes as input angle references, like a crazyflie entity with input angles
+ angleControlledCrazyflie(stateInertialEstimate, input_angle, 1, response);
- // ***********************************************************
- // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
- // D D E B B U U G MM MM S G
- // D D EEE BBBB U U G M M M SSS G
- // D D E B B U U G G M M S G G
- // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
- // DEBUGGING CODE:
- // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
- // By fill this message with data and publishing it you can display the data in
- // real time using rpt plots. Instructions for using rqt plots can be found on
- // the wiki of the D-FaLL-System repository
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
- // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
- // (located in the "msg" folder) to see the full structure of this message.
- DebugMsg debugMsg;
- // Fill the debugging message with the data provided by Vicon
- debugMsg.vicon_x = request.ownCrazyflie.x;
- debugMsg.vicon_y = request.ownCrazyflie.y;
- debugMsg.vicon_z = request.ownCrazyflie.z;
- debugMsg.vicon_roll = request.ownCrazyflie.roll;
- debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
- debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
- // Fill in the debugging message with any other data you would like to display
- // in real time. For example, it might be useful to display the thrust
- // adjustment computed by the z-altitude controller.
- // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
- // type "float64" that you can fill in with data you would like to plot in
- // real-time.
- // debugMsg.value_1 = thrustAdjustment;
- // ......................
- // debugMsg.value_10 = your_variable_name;
- // Publish the "debugMsg"
- debugPublisher.publish(debugMsg);
- // An alternate debugging technique is to print out data directly to the
- // command line from which this node was launched.
+void perform_estimator_update_state_interial(Controller::Request &request, float (&stateInertialEstimate)[12])
+{
- // An example of "printing out" the data from the "request" argument to the
- // command line. This might be useful for debugging.
- // ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
- // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
- // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
- // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
- // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
- // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
- // ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
- // An example of "printing out" the control actions computed.
- // ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- // ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- // ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- // ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
- // ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // RUN THE FINITE DIFFERENCE ESTIMATOR and fill stateEstimate with result
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(stateInertialEstimate);
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR and fill stateEstimate with result
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(stateInertialEstimate);
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default and fill stateEstimate with result
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(stateInertialEstimate);
+ break;
+ }
+ }
- // An example of "printing out" the per motor 16-bit command computed.
- // ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
- // ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
- // ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
- // ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
- // Return "true" to indicate that the control computation was performed successfully
- return true;
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
}
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12])
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInertialEstimate[0] = current_xzy_rpy_measurement[0];
+ stateInertialEstimate[1] = current_xzy_rpy_measurement[1];
+ stateInertialEstimate[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInertialEstimate[6] = current_xzy_rpy_measurement[3];
+ stateInertialEstimate[7] = current_xzy_rpy_measurement[4];
+ stateInertialEstimate[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInertialEstimate[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInertialEstimate[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInertialEstimate[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInertialEstimate[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInertialEstimate[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInertialEstimate[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
-// ------------------------------------------------------------------------------
-// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
-// R R O O T A A T E I NN N T O O
-// RRRR O O T A A T EEE I N N N T O O
-// R R O O T AAAAA T E I N NN T O O
-// R R OOO T A A T EEEEE III N N T OOO
-//
-// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
-// B B O O D D Y Y F R R A A MM MM E
-// BBBB O O D D Y FFF RRRR A A M M M EEE
-// B B O O D D Y F R R AAAAA M M E
-// BBBB OOO DDDD Y F R R A A M M EEEEE
-// ----------------------------------------------------------------------------------
-
-// The arguments for this function are as follows:
-// stateInertial
-// This is an array of length 9 with the estimates the error of of the following values
-// relative to the sepcifed setpoint:
-// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
-// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
-// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
-//
-// stateBody
-// This is an empty array of length 9, this function should fill in all elements of this
-// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
-// position and (x,y) velocities are rotated into the body frame.
-//
-// yaw_measured
-// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
-// the Vicon motion capture system
-//
-// This function WILL NEED TO BE edited for successful completion of the PPS exercise
-void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12])
{
- // Fill in the (x,y,z) position estimates to be returned
- stateBody[0] = stateInertial[0];
- stateBody[1] = stateInertial[1];
- stateBody[2] = stateInertial[2];
-
- // Fill in the (x,y,z) velocity estimates to be returned
- stateBody[3] = stateInertial[3];
- stateBody[4] = stateInertial[4];
- stateBody[5] = stateInertial[5];
-
- // Fill in the (roll,pitch,yaw) estimates to be returned
- stateBody[6] = stateInertial[6];
- stateBody[7] = stateInertial[7];
- stateBody[8] = stateInertial[8];
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInertialEstimate[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInertialEstimate[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInertialEstimate[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInertialEstimate[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInertialEstimate[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInertialEstimate[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInertialEstimate[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInertialEstimate[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInertialEstimate[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInertialEstimate[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInertialEstimate[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInertialEstimate[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInertialEstimate[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+
}
+
// ------------------------------------------------------------------------------
// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
// NN N E W W T O O NN N C MM MM D D
@@ -776,30 +708,45 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Here we load the parameters that are specified in the CustomController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_customController(nodeHandle, "MpcController");
+ ros::NodeHandle nodeHandle_for_MpcController(nodeHandle, "MpcController");
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
+ cf_mass = getParameterFloat(nodeHandle_for_MpcController , "mass");
// Display one of the YAML parameters to debug if it is working correctly
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_customController, "control_frequency");
+ control_frequency = getParameterFloat(nodeHandle_for_MpcController, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_customController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
+ getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
// Call the function that computes details an values that are needed from these
// parameters loaded above
processFetchedParameters();
-
}
@@ -811,7 +758,6 @@ void processFetchedParameters()
// Compute the feed-forward force that we need to counteract gravity (i.e., mg)
// > in units of [Newtons]
gravity_force = cf_mass * 9.81/(1000*4);
-
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
}