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

added funSxSuSg_varying_affine

parent ccc66c7a
......@@ -30,3 +30,5 @@ void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, para
void discretization_affine(const MatrixAtype A, const MatrixBtype B, const VectorXtype x_tray, const VectorUtype u_tray, params_t params, MatrixAtype &A_d, MatrixBtype &B_d, VectorXtype &g_d);
void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vector<VectorUtype> U_tray, params_t params, std::vector<MatrixAtype> &A_tray, std::vector<MatrixBtype> &B_tray, std::vector<VectorXtype> &g_tray);
void euler_method_forward(F_callback_type pF, float t0, float h, float tfinal, VectorXtype y0, VectorUtype u, params_t params, std::vector<VectorXtype> &yout);
void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, int N, Eigen::MatrixXf &Sx, Eigen::MatrixXf &Su, Eigen::MatrixXf &Sg);
......@@ -121,13 +121,130 @@ void euler_method_forward(F_callback_type pF, float t0, float h, float tfinal, V
{
VectorXtype y = y0;
VectorXtype y_next;
yout.push_back(y);
std::vector<VectorXtype> yout_temp;
yout_temp.push_back(y);
for(float t = t0; t < tfinal; t = t + h)
{
pF(y, u, params, y_next);
y = y + h*y_next;
yout.push_back(y);
yout_temp.push_back(y);
}
yout = yout_temp;
}
void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, int N, MatrixXf &Sx, MatrixXf &Su, MatrixXf &Sg)
{
int N_A = A_tray.size();
int N_B = B_tray.size();
if(N_A != N)
std::cout << "Error: number of A matrices hsould be equal to N"<< endl << endl;
if(N_B != N)
std::cout << "Error: number of A matrices hsould be equal to N"<< endl << endl;
// int r_A = A_tray[0].rows();
// int c_A = A_tray[0].cols();
// int r_A = B_tray[0].rows();
// int c_A = B_tray[0].cols();
int r_A = N_x;
int c_A = N_x;
int r_B = N_x;
int c_B = N_u;
// S_x is the temporal, local variable that will in the end be the output, Sx
MatrixXf S_x = MatrixXf::Zero(r_A*(N+1), c_A);
MatrixAtype A_mult = MatrixXf::Identity(r_A, r_A);
// construct S_x:
for(int i = 0; i < N+1; i++)
{
S_x.middleRows(i*r_A, r_A) << A_mult;
if (i < N) // we cannot access index N in A_tray, protect it
{
A_mult = A_tray[i]*A_mult;
}
}
// construct Su
MatrixXf S_u = MatrixXf::Zero(r_B*(N+1), c_B*N);
// temporal variables:
MatrixXf column_Su;
MatrixAtype mult;
for(int i = 0; i < N; i++)
{
column_Su = MatrixXf::Ones(r_B * (N+1), c_B);
mult = MatrixXf::Identity(r_A, c_A);
for(int j = 0; j < N+1; j++)
{
if (j == i+1)
{
column_Su.middleRows(r_B*j, r_B) << B_tray[i];
}
else if (j > i+1)
{
mult = A_tray[j-1] * mult;
column_Su.middleRows(r_B*j, r_B) << mult * B_tray[i];
}
else
{
// can possibly omit this else, just for clarity. Of course first we need to initizlize to zero instead of ones columns Su
column_Su.middleRows(r_B*j, r_B).setZero();
}
}
S_u.middleCols(c_B*i, c_B) << column_Su;
}
// build Sg matrix
MatrixXf S_g = MatrixXf::Zero(r_A*(N+1), c_A*N);
// temporal variables:
MatrixXf column_Sg;
for(int i = 0; i < N; i++)
{
column_Sg = MatrixXf::Ones(r_A * (N+1), c_A);
mult = MatrixXf::Identity(r_A, c_A);
for(int j = 0; j < N+1; j++)
{
if (j == i+1)
{
column_Sg.middleRows(r_A*j, r_A).setIdentity();
}
else if (j > i+1)
{
mult = A_tray[j-1] * mult;
column_Sg.middleRows(r_A*j, r_A) << mult;
}
else
{
// can possibly omit this else, just for clarity. Of course first we need to initizlize to zero instead of ones columns Su
column_Sg.middleRows(r_A*j, r_A).setZero();
}
}
S_g.middleCols(c_A*i, c_A) << column_Sg;
}
Sx = S_x;
Su = S_u;
Sg = S_g;
}
......@@ -683,14 +683,15 @@ void setpointCallback(const Setpoint& newSetpoint)
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
for(int i = 0; i < g_tray.size(); i++)
for(int i = 0; i < A_tray.size(); i++)
{
std::cout << "index matrix" << i << std::endl;
std::cout << g_tray[i] << std::endl;
std::cout << A_tray[i] << std::endl;
}
// debug
Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// std::cout << A.format(OctaveFmt) << std::endl;
......@@ -712,6 +713,25 @@ void setpointCallback(const Setpoint& newSetpoint)
std::cout << x_out[i] << std::endl;
}
Eigen::MatrixXf Sx_test;
Eigen::MatrixXf Su_test;
Eigen::MatrixXf Sg_test;
int N = 5;
funSxSuSg_varying_affine(A_tray, B_tray, N, Sx_test, Su_test, Sg_test);
std::cout << "Sx test:" << std::endl << std::endl;
std::cout << Sx_test<< std::endl;
std::cout << "Su test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Su_test.format(CleanFmt) << std::endl;
std::cout << "Sg test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Sg_test.format(CleanFmt) << std::endl;
}
......
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