From ee6ac0f659fd1f3964aa3c7a8f24f9a8aad83e8b Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Fri, 27 Mar 2020 11:31:42 +0100
Subject: [PATCH] Updated the csone picker remote template and tuning
controllers to correctly respond to the is first controller call flag
---
.../include/nodes/PickerControllerService.h | 4 +-
.../include/nodes/RemoteControllerService.h | 20 +--
.../include/nodes/TuningControllerService.h | 20 +--
.../src/nodes/CsoneControllerService.cpp | 21 ++-
.../src/nodes/PickerControllerService.cpp | 58 +++++--
.../src/nodes/RemoteControllerService.cpp | 144 +++++++++-------
.../src/nodes/TemplateControllerService.cpp | 21 ++-
.../src/nodes/TuningControllerService.cpp | 154 +++++++++++-------
8 files changed, 283 insertions(+), 159 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
index edb52d6f..a4c09f19 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
@@ -345,8 +345,8 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
index 2b9e5899..a47c52b5 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
@@ -274,7 +274,7 @@ float gravity_force_quarter = 0.25 * gravity_force;
// VARIABLES FOR THE CONTROLLER
// Frequency at which the controller is running
-float vicon_frequency = 200.0;
+float yaml_vicon_frequency = 200.0;
// Frequency at which the controller is running
float control_frequency = 200.0;
@@ -324,29 +324,29 @@ float cmd_sixteenbit_max = 60000;
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
-float estimator_frequency = 200.0;
+float m_estimator_frequency = 200.0;
// > A flag for which estimator to use:
-int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// > The current state interial estimate,
// for use by the controller
-float current_stateInertialEstimate[12];
+float m_current_stateInertialEstimate[12];
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
-float current_xzy_rpy_measurement[6];
+float m_current_xzy_rpy_measurement[6];
// > The measurement of the Crazyflie at the "previous" time step,
// used for computing finite difference velocities
-float previous_xzy_rpy_measurement[6];
+float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
-float stateInterialEstimate_viaFiniteDifference[12];
+float m_stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
-float stateInterialEstimate_viaPointMassKalmanFilter[12];
+float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
@@ -451,8 +451,8 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
index 6172fb86..5534b1ba 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
@@ -288,7 +288,7 @@ float gravity_force_quarter;
// VARIABLES FOR THE CONTROLLER
// Frequency at which the controller is running
-float vicon_frequency;
+float yaml_vicon_frequency;
// Frequency at which the controller is running
float control_frequency;
@@ -362,29 +362,29 @@ float cmd_sixteenbit_max;
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
-float estimator_frequency;
+float m_estimator_frequency;
// > A flag for which estimator to use:
-int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// > The current state interial estimate,
// for use by the controller
-float current_stateInertialEstimate[12];
+float m_current_stateInertialEstimate[12];
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
-float current_xzy_rpy_measurement[6];
+float m_current_xzy_rpy_measurement[6];
// > The measurement of the Crazyflie at the "previous" time step,
// used for computing finite difference velocities
-float previous_xzy_rpy_measurement[6];
+float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
-float stateInterialEstimate_viaFiniteDifference[12];
+float m_stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
-float stateInterialEstimate_viaPointMassKalmanFilter[12];
+float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
@@ -489,8 +489,8 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index c96140f4..cc508b98 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -218,11 +218,22 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity
- // > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
-
+ // > But only if this is NOT the first call to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute as simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
+
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
stateErrorInertial[7] = request.ownCrazyflie.pitch;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index f1b3c85a..b30281bf 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -458,11 +458,11 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
@@ -517,7 +517,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
@@ -530,19 +530,33 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
// COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
- // > for (x,y,z) velocities
- m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
- // > for (roll,pitch,yaw) velocities
- m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_estimator_frequency;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > for (x,y,z) velocities
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_estimator_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[9] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[10] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[11] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// PERFORM THE KALMAN FILTER UPDATE STEP
// > First take a copy of the estimator state
@@ -551,6 +565,26 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
{
temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ // Set the angles to the current measurement
+ temp_PMKFstate[6] = m_current_xzy_rpy_measurement[3];
+ temp_PMKFstate[7] = m_current_xzy_rpy_measurement[4];
+ temp_PMKFstate[8] = m_current_xzy_rpy_measurement[5];
+ // Set the velocities to zero
+ temp_PMKFstate[9] = 0.0;
+ temp_PMKFstate[10] = 0.0;
+ temp_PMKFstate[11] = 0.0;
+ }
// > Now perform update for:
// > x position and velocity:
m_stateInterialEstimate_viaPointMassKalmanFilter[0] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index a2887e62..e1e01f28 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -174,7 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
- // "current_stateInertialEstimate" is updated and ready
+ // "m_current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
@@ -184,7 +184,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (isActive_remoteControlMode)
{
// CARRY OUT THE CONTROLLER COMPUTATIONS
- calculateControlOutput_forRemoteControl(current_stateInertialEstimate,request,response);
+ calculateControlOutput_forRemoteControl(m_current_stateInertialEstimate,request,response);
}
else
@@ -194,7 +194,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > Define a local array to fill in with the body frame error
float current_bodyFrameError[12];
// > Call the function to perform the conversion
- convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+ convert_stateInertial_to_bodyFrameError(m_current_stateInertialEstimate,setpoint,current_bodyFrameError);
@@ -481,25 +481,25 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// 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;
+ m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ m_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;
+ m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
// RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (estimator_method)
+ switch (yaml_estimator_method)
{
// Estimator based on finte differences
case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
@@ -507,7 +507,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -517,19 +517,19 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
break;
}
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
- ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'estimator_method' is not recognised.");
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'yaml_estimator_method' is not recognised.");
// Transfer the finite difference estimate by default
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -539,71 +539,105 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// 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];
+ m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+ m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+ m_previous_xzy_rpy_measurement[2] = m_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];
+ m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+ m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+ m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
}
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
- stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
- stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
- stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaFiniteDifference[0] = m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaFiniteDifference[1] = m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaFiniteDifference[2] = m_current_xzy_rpy_measurement[2];
// > for (roll,pitch,yaw) angles
- stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
- stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
- stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaFiniteDifference[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaFiniteDifference[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
// COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
- // > for (x,y,z) velocities
- stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
- // > for (roll,pitch,yaw) velocities
- stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > for (x,y,z) velocities
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_estimator_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[9] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[10] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[11] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// 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] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ // Set the angles to the current measurement
+ temp_PMKFstate[6] = m_current_xzy_rpy_measurement[3];
+ temp_PMKFstate[7] = m_current_xzy_rpy_measurement[4];
+ temp_PMKFstate[8] = m_current_xzy_rpy_measurement[5];
+ // Set the velocities to zero
+ temp_PMKFstate[9] = 0.0;
+ temp_PMKFstate[10] = 0.0;
+ temp_PMKFstate[11] = 0.0;
}
// > Now perform update for:
// > x position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[0];
// > y position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[1];
// > z position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[2];
// > roll position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[3];
// > pitch position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[4];
// > yaw position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[5];
}
@@ -1361,7 +1395,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- vicon_frequency = getParameterFloat(nodeHandle_for_paramaters, "vicon_frequency");
+ yaml_vicon_frequency = getParameterFloat(nodeHandle_for_paramaters, "vicon_frequency");
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
@@ -1387,7 +1421,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
controller_mode = getParameterInt( nodeHandle_for_paramaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
@@ -1438,7 +1472,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
gravity_force_quarter = cf_mass * 9.81/(1000.0*4.0);
// Set that the estimator frequency is the same as the control frequency
- estimator_frequency = vicon_frequency;
+ m_estimator_frequency = yaml_vicon_frequency;
// Roll and pitch limit (in degrees for angles)
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
index 99010a99..535d907c 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
@@ -191,11 +191,22 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity
- // > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
-
+ // > But only if this is NOT the first call to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute as simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
+
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
stateErrorInertial[7] = request.ownCrazyflie.pitch;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
index f02aad16..d18bd7ee 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
@@ -174,7 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
- // "current_stateInertialEstimate" is updated and ready
+ // "m_current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
@@ -184,7 +184,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > Define a local array to fill in with the body frame error
float current_bodyFrameError[12];
// > Call the function to perform the conversion
- convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+ convert_stateInertial_to_bodyFrameError(m_current_stateInertialEstimate,setpoint,current_bodyFrameError);
// UPDATE THE SETPOINT FOR FLYING IN A CIRCLE - IF ACTIVE
@@ -273,7 +273,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_horizontal_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_horizontal_currentpoint' is not recognised.");
break;
}
@@ -319,7 +319,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_vertical_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_vertical_currentpoint' is not recognised.");
break;
}
@@ -365,7 +365,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_heading_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_heading_currentpoint' is not recognised.");
break;
}
@@ -411,7 +411,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_all_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_all_currentpoint' is not recognised.");
break;
}
@@ -442,7 +442,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_index" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_index' is not recognised.");
break;
}
@@ -453,7 +453,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
void update_setpoint_for_test_circle()
{
// Compute the time since start
- float time_since_start = float(test_circle_ticks_since_start) / vicon_frequency;
+ float time_since_start = float(test_circle_ticks_since_start) / yaml_vicon_frequency;
// Allow time to reach the start point
if (time_since_start < test_circle_time_to_reach_start)
@@ -550,25 +550,25 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// 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;
+ m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ m_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;
+ m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
// RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (estimator_method)
+ switch (yaml_estimator_method)
{
// Estimator based on finte differences
case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
@@ -576,7 +576,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -586,19 +586,19 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
break;
}
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
- ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'estimator_method' is not recognised.");
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'yaml_estimator_method' is not recognised.");
// Transfer the finite difference estimate by default
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -608,71 +608,105 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// 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];
+ m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+ m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+ m_previous_xzy_rpy_measurement[2] = m_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];
+ m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+ m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+ m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
}
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
- stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
- stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
- stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaFiniteDifference[0] = m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaFiniteDifference[1] = m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaFiniteDifference[2] = m_current_xzy_rpy_measurement[2];
// > for (roll,pitch,yaw) angles
- stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
- stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
- stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaFiniteDifference[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaFiniteDifference[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
// COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
- // > for (x,y,z) velocities
- stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
- // > for (roll,pitch,yaw) velocities
- stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
- stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > for (x,y,z) velocities
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_estimator_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[9] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[10] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[11] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// 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] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ // Set the angles to the current measurement
+ temp_PMKFstate[6] = m_current_xzy_rpy_measurement[3];
+ temp_PMKFstate[7] = m_current_xzy_rpy_measurement[4];
+ temp_PMKFstate[8] = m_current_xzy_rpy_measurement[5];
+ // Set the velocities to zero
+ temp_PMKFstate[9] = 0.0;
+ temp_PMKFstate[10] = 0.0;
+ temp_PMKFstate[11] = 0.0;
}
// > Now perform update for:
// > x position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[0];
// > y position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[1];
// > z position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[2];
// > roll position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[3];
// > pitch position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[4];
// > yaw position and velocity:
- stateInterialEstimate_viaPointMassKalmanFilter[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];
- stateInterialEstimate_viaPointMassKalmanFilter[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];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[5];
}
@@ -1494,7 +1528,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- vicon_frequency = getParameterFloat(nodeHandle_for_paramaters, "vicon_frequency");
+ yaml_vicon_frequency = getParameterFloat(nodeHandle_for_paramaters, "vicon_frequency");
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
@@ -1520,7 +1554,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
controller_mode = getParameterInt( nodeHandle_for_paramaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
@@ -1573,7 +1607,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
gravity_force_quarter = cf_mass * 9.81/(1000*4);
// Set that the estimator frequency is the same as the control frequency
- estimator_frequency = vicon_frequency;
+ m_estimator_frequency = yaml_vicon_frequency;
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: after processing the gravity_force_quarter = " << gravity_force_quarter);
--
GitLab