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

added funRbar and funQbar. Tested

parent 49565ced
......@@ -32,3 +32,7 @@ void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vec
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);
void funQbar(Eigen::MatrixXf Q, Eigen::MatrixXf P, int N, Eigen::MatrixXf &Qbar);
void funRbar(Eigen::MatrixXf R, int N, Eigen::MatrixXf &Rbar);
#include "MPC_functions.h"
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>
using namespace Eigen;
using namespace std;
......@@ -178,13 +179,12 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
// 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);
A_mult = MatrixXf::Identity(r_A, c_A);
for(int j = 0; j < N+1; j++)
{
......@@ -195,8 +195,8 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
}
else if (j > i+1)
{
mult = A_tray[j-1] * mult;
column_Su.middleRows(r_B*j, r_B) << mult * B_tray[i];
A_mult = A_tray[j-1] * A_mult;
column_Su.middleRows(r_B*j, r_B) << A_mult * B_tray[i];
}
else
{
......@@ -220,7 +220,7 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
for(int i = 0; i < N; i++)
{
column_Sg = MatrixXf::Ones(r_A * (N+1), c_A);
mult = MatrixXf::Identity(r_A, c_A);
A_mult = MatrixXf::Identity(r_A, c_A);
for(int j = 0; j < N+1; j++)
{
......@@ -231,8 +231,8 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
}
else if (j > i+1)
{
mult = A_tray[j-1] * mult;
column_Sg.middleRows(r_A*j, r_A) << mult;
A_mult = A_tray[j-1] * A_mult;
column_Sg.middleRows(r_A*j, r_A) << A_mult;
}
else
{
......@@ -248,3 +248,23 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
Su = S_u;
Sg = S_g;
}
void funQbar(MatrixXf Q, MatrixXf P, int N, MatrixXf &Qbar)
{
MatrixXf Q_bar;
MatrixXf diag_mask = VectorXf::Ones(N+1).asDiagonal();
Q_bar = kroneckerProduct(diag_mask, Q);
Q_bar.bottomRightCorner(P.rows(), P.cols()) = P;
Qbar = Q_bar;
}
void funRbar(MatrixXf R, int N, MatrixXf &Rbar)
{
MatrixXf R_bar;
MatrixXf diag_mask = VectorXf::Ones(N).asDiagonal();
R_bar = kroneckerProduct(diag_mask, R);
Rbar = R_bar;
}
......@@ -731,7 +731,32 @@ void setpointCallback(const Setpoint& newSetpoint)
// std::cout << Su_test<< std::endl;
std::cout << Sg_test.format(CleanFmt) << std::endl;
Eigen::MatrixXf Qbar;
Eigen::MatrixXf Rbar;
VectorXtype p_vec;
p_vec << 3, 3, 3, 2, 2, 2;
VectorXtype q_vec;
q_vec << 1, 1, 1, 4, 4, 4;
VectorUtype r_vec;
r_vec << 1, 1, 1, 4;
Eigen::MatrixXf P = p_vec.asDiagonal();
Eigen::MatrixXf Q = q_vec.asDiagonal();
Eigen::MatrixXf R = r_vec.asDiagonal();
funQbar(Q, P, N, Qbar);
std::cout << "Qbar test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Qbar.format(CleanFmt) << std::endl;
funRbar(R,N, Rbar);
std::cout << "Rbar test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Rbar.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