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 6352d279 authored by roangel's avatar roangel
Browse files

added MPC variables initialization

parent 14fcbaed
......@@ -19,7 +19,7 @@ typedef struct params_t {
float m;
float g;
float Ts;
} params;
} params_TEMPLATE;
typedef void (*F_callback_type)(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec);
......
......@@ -162,6 +162,28 @@ 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
// Variables for MPC:
Eigen::MatrixXd P(N_x,N_x);
Eigen::MatrixXd Q(N_x,N_x);
Eigen::MatrixXd R(N_u,N_u);
Eigen::VectorXd X_ref;
Eigen::VectorXd U_ref;
std::vector<MatrixAtype> A_tray;
std::vector<MatrixBtype> B_tray;
std::vector<VectorXtype> g_tray;
std::vector<VectorXtype> X_tray;
std::vector<VectorUtype> U_tray;
params_t params;
VectorXtype x0;
int N = 5;
// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
......@@ -242,7 +264,7 @@ void perform_estimator_update_state_interial(Controller::Request &request, float
void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12]);
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12]);
void initialize_MPC_variables(int N);
// ----------------------------------------------------------------------------------
......@@ -652,9 +674,6 @@ void setpointCallback(const Setpoint& newSetpoint)
MatrixBtype B_d;
VectorXtype g_d;
params_t params;
params.m = cf_mass/1000.0;
params.g = 9.81;
params.Ts = 1/50.0;
......@@ -778,6 +797,60 @@ void setpointCallback(const Setpoint& newSetpoint)
Eigen::VectorXd U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 0);
}
void initialize_MPC_variables(int N)
{
// params:
params.m = cf_mass/1000.0;
params.g = 9.81;
params.Ts = 1/50.0;
// Initialize initial state, 6 states
x0 << -setpoint[0], -setpoint[1], -setpoint[2], 0, 0, 0;
VectorUtype u_temp;
u_temp << 0, 0, 0, params.m*params.g;
// X_tray and U_tray
for(int i = 0; i < N; i++)
{
X_tray.push_back(x0);
U_tray.push_back(u_temp);
}
// fill A_tray, B_tray, g_tray with initial values
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
VectorXtype q_vec;
q_vec << 5000, 5000, 5000, 500, 500, 500;
VectorUtype r_vec;
r_vec << 2000, 2000, 10000, 150000;
Q = q_vec.asDiagonal();
R = r_vec.asDiagonal();
P <<97395, 0 , 0 , 12731, 0 , 0 ,
0 , 97395, 0 , 0 , 12731, 0 ,
0 , 0 , 124811, 0 , 0 , 31321,
12731, 0 , 0 , 7027 , 0 , 0 ,
0 , 12731, 0 , 0 , 7027 , 0 ,
0 , 0 , 31321 , 0 , 0 , 22707;
// X_ref and U_ref. These don't change at all
X_ref.resize(N_x*(N+1));
U_ref.resize(N_u*N);
VectorXtype x_ref;
x_ref << 0,0,0,0,0,0;
VectorUtype u_ref;
u_ref << 0,0,0,params.m*params.g;
for(int i = 0; i < N+1; i++)
{
X_ref.segment(i*N_x, N_x) = x_ref;
if(i < N)
U_ref.segment(i*N_u, N_u) = u_ref;
}
}
......
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