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 f3d16543 authored by beuchatp's avatar beuchatp
Browse files

check estimation error

parent 0b4fd4c8
......@@ -287,11 +287,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
// update measurement vector using vicon Data
get_y_from_CFData( request );
// compute distributed controller commands
distributed_control_command ( request, response );
}
// compute distributed controller commands
distributed_control_command ( request, response );
break;
}
......@@ -466,9 +467,34 @@ void distributed_control_command( Controller::Request &request, Controller::Resp
outYaw = u(11,0);
}
if (outer_loop_counter % 100 == 0)
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
}
if (outer_loop_counter % 1 == 0)
{
float est_error_roll = overlapping_estimator_state_prev(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = overlapping_estimator_state_prev(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = overlapping_estimator_state_prev(2)-request.otherCrazyflies[0].yaw;
float est_error_load_x = overlapping_estimator_state_prev(7)-request.otherCrazyflies[3].x;
float est_error_load_y = overlapping_estimator_state_prev(8)-request.otherCrazyflies[3].y;
float est_error_load_z = overlapping_estimator_state_prev(9)-request.otherCrazyflies[3].z;
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATION ERRORS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll = " << est_error_roll);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch = " << est_error_pitch);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw = " << est_error_yaw);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: x = " << est_error_load_x);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: y = " << est_error_load_y);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: z = " << est_error_load_z);
ROS_INFO_STREAM("[STUDENT CONTROLLER] CABLE ANGLE ESTIMATES:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << overlapping_estimator_state_prev(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << overlapping_estimator_state_prev(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << overlapping_estimator_state_prev(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << overlapping_estimator_state_prev(6) );
}
// PREPARE AND RETURN THE VARIABLE "response"
......@@ -515,20 +541,26 @@ void display_control_agent1( Controller::Request &request, Controller::Response
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: roll estimate = " << estimator_state_prev_only_agent1[0] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: pitch estimate = " << estimator_state_prev_only_agent1[1] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: yaw estimate = " << estimator_state_prev_only_agent1[2] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi estimate = " << estimator_state_prev_only_agent1[3] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable theta estimate = " << estimator_state_prev_only_agent1[4] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi_dot estimate = " << estimator_state_prev_only_agent1[5] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1[6] );
float est_error_roll = estimator_state_prev_only_agent1(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = estimator_state_prev_only_agent1(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = estimator_state_prev_only_agent1(2)-request.otherCrazyflies[0].yaw;
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATION ERRORS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll = " << est_error_roll);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch = " << est_error_pitch);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw = " << est_error_yaw);
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATES:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: roll estimate = " << estimator_state_prev_only_agent1(0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: pitch estimate = " << estimator_state_prev_only_agent1(1) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: yaw estimate = " <<estimator_state_prev_only_agent1(2) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi estimate = " << estimator_state_prev_only_agent1(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable theta estimate = " << estimator_state_prev_only_agent1(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi_dot estimate = " << estimator_state_prev_only_agent1(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1(6) );
}
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " << outPitch );
......@@ -538,7 +570,6 @@ void display_control_agent1( Controller::Request &request, Controller::Response
// Debugging
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_only_agent_1(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_only_agent_1(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_only_agent_1(2,0) );
......@@ -633,14 +664,24 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
}
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll estimate = " << estimator_state_prev[0] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch estimate = " << estimator_state_prev[1] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw estimate = " << estimator_state_prev[2] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev[3] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev[4] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev[5] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev[6] );
float est_error_roll = overlapping_estimator_state_prev(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = overlapping_estimator_state_prev(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = overlapping_estimator_state_prev(2)-request.otherCrazyflies[0].yaw;
float est_error_load_x = overlapping_estimator_state_prev(21)-request.otherCrazyflies[3].x;
float est_error_load_y = overlapping_estimator_state_prev(22)-request.otherCrazyflies[3].y;
float est_error_load_z = overlapping_estimator_state_prev(23)-request.otherCrazyflies[3].z;
ROS_INFO_STREAM("[STUDENT CONTROLLER] ESTIMATION ERRORS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll = " << est_error_roll);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch = " << est_error_pitch);
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw = " << est_error_yaw);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: x = " << est_error_load_x);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: y = " << est_error_load_y);
ROS_INFO_STREAM("[STUDENT CONTROLLER] load: z = " << est_error_load_z);
ROS_INFO_STREAM("[STUDENT CONTROLLER] CABLE ANGLE ESTIMATES:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev(6) );
}
......@@ -1696,7 +1737,7 @@ int main(int argc, char* argv[]) {
// get current working directory for centralized controller
std::string folderpath_c = getcwd_string();
folderpath_c = folderpath_c.substr(0, folderpath_c.size()-5);
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/centralized_gains/load_measurements_qu1e-3/");
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/centralized_gains/load_measurements/");
// READ MATRICES FROM DATA FOLDER
std::string filename_K = folderpath;
......
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