Commit 87906276 authored by beuchatp's avatar beuchatp
Browse files

Added the Point Mass Kalman Filter (needs testing)

parent 3018e23e
......@@ -194,16 +194,10 @@ int lqr_controller_mode = LQR_MODE_RATE;
// The LQR Controller parameters for "LQR_MODE_MOTOR"
std::vector<float> gainMatrixMotor1;
std::vector<float> gainMatrixMotor2;
std::vector<float> gainMatrixMotor3;
std::vector<float> gainMatrixMotor4;
std::vector<float> gainMatrixMotor1 (12,0.0);
std::vector<float> gainMatrixMotor2 (12,0.0);
std::vector<float> gainMatrixMotor3 (12,0.0);
std::vector<float> gainMatrixMotor4 (12,0.0);
// The LQR Controller parameters for "LQR_MODE_ACTUATOR"
std::vector<float> gainMatrixThrust_TwelveStateVector = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00, 0.00, 0.00, 0.00};
......@@ -211,22 +205,18 @@ std::vector<float> gainMatrixRollTorque = { 1.72, 0.00, 0.00, 1.
std::vector<float> gainMatrixPitchTorque = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84, 0.00, 0.00, 0.00};
std::vector<float> gainMatrixYawTorque = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
// The LQR Controller parameters for "LQR_MODE_RATE"
std::vector<float> gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
std::vector<float> gainMatrixRollRate = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
std::vector<float> gainMatrixPitchRate = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
// The LQR Controller parameters for "LQR_MODE_ANGLE"
std::vector<float> gainMatrixThrust_SixStateVector = { 0.00, 0.00, 0.31, 0.00, 0.00, 0.14};
std::vector<float> gainMatrixRollAngle = { 0.00,-0.20, 0.00, 0.00,-0.20, 0.00};
std::vector<float> gainMatrixPitchAngle = { 0.20, 0.00, 0.00, 0.20, 0.00, 0.00};
// The 16-bit command limits
float cmd_sixteenbit_min;
float cmd_sixteenbit_max;
......@@ -255,18 +245,32 @@ float previous_xzy_rpy_measurement[6];
// difference state estimator
float stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
float stateInterialEstimate_viaPointMassKalmanFilter[12];
// 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)
// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
// Variable for the namespaces for the paramter services
// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
ros::Publisher debugPublisher;
// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
......@@ -370,6 +374,7 @@ void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Control
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
......
......@@ -29,6 +29,7 @@ shouldPublishDebugMessage : false
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : true
# A flag for which LQR controller mode to use, defined as:
# 1 - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
# commands per motor thrusts
......@@ -36,7 +37,7 @@ shouldDisplayDebugInfo : true
# commands actuators of total force and bodz torques
# 3 - LQR controller based on the state vector: [position,velocity,angles]
# 4 - LQR controller based on the state vector: [position,velocity]
lqr_controller_mode : 1
lqr_controller_mode : 3
# A flag for which estimator to use, defined as:
......@@ -52,43 +53,42 @@ lqr_controller_mode : 1
estimator_method : 1
# The LQR Controller parameters for "mode = 1"
gainMatrixMotor1 : [ -0.0620, 0.0620, 0.0496,-0.0445, 0.0445, 0.0282,-0.1567,-0.1568,-0.0688,-0.0063,-0.0063,-0.0130]
gainMatrixMotor2 : [ 0.0620, 0.0620, 0.0496, 0.0445, 0.0445, 0.0282,-0.1567, 0.1568, 0.0688,-0.0063, 0.0063, 0.0130]
gainMatrixMotor3 : [ 0.0620,-0.0620, 0.0496, 0.0445,-0.0445, 0.0282, 0.1567, 0.1568,-0.0688, 0.0063, 0.0063,-0.0130]
gainMatrixMotor4 : [ -0.0620,-0.0620, 0.0496,-0.0445,-0.0445, 0.0282, 0.1567,-0.1568, 0.0688, 0.0063,-0.0063, 0.0130]
# The LQR Controller parameters for "mode = 2"
gainMatrixThrust_TwelveStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00]
gainMatrixRollTorque : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00, 0.00, 0.00, 0.00]
gainMatrixPitchTorque : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00, 0.00, 0.00, 0.00]
gainMatrixYawTorque : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30, 0.00, 0.00, 0.00]
# The LQR Controller parameters for "mode = 3"
gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
# The LQR Controller parameters for "mode = 4"
gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.31, 0.00, 0.00, 0.14]
gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
# The max and minimum thrust for a 16-bit command
command_sixteenbit_min : 1000
command_sixteenbit_max : 60000
\ No newline at end of file
command_sixteenbit_max : 60000
# 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.5896, 0.0029]
PMKF_Ahat_row2_for_angles : [-21.5535, 0.8922]
PMKF_Kinf_for_angles : [ 0.4104,21.5535]
......@@ -230,10 +230,14 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
// RUN THE FINITE DIFFERENCE ESTIMATOR
performEstimatorUpdate_forStateInterial_viaFiniteDifference();
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
switch (estimator_method)
......@@ -248,6 +252,17 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
}
break;
}
// Estimator based on Point Mass Kalman Filter
case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
{
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
}
break;
}
// Handle the exception
default:
{
// Display that the "estimator_method" was not recognised
......@@ -275,6 +290,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
}
void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
......@@ -300,6 +316,41 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
{
// 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];
}
// > 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];
// > 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];
// > 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];
// > 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];
// > 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];
// > 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];
}
// ----------------------------------------------------------------------------------
// L QQQ RRRR
// L Q Q R R
......@@ -1085,6 +1136,18 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_demoController, "command_sixteenbit_min");
cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_demoController, "command_sixteenbit_max");
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("DEBUGGING: the fetched SafeController/mass = " << cf_mass);
......
Supports Markdown
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