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

added euler method function, also tested

parent 929affc5
......@@ -14,14 +14,19 @@ typedef Eigen::Matrix<float, N_x, N_u> MatrixBtype;
typedef Eigen::Matrix<float, N_x, 1> VectorXtype;
typedef Eigen::Matrix<float, N_u, 1> VectorUtype;
typedef struct params_t {
float m;
float g;
float Ts;
} params;
typedef void (*F_callback_type)(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec);
void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, const params_t params, MatrixAtype &A, MatrixBtype &B);
void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec);
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);
......@@ -114,3 +114,20 @@ void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vec
g_tray.push_back(g_temp);
}
}
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)
{
VectorXtype y = y0;
VectorXtype y_next;
yout.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);
}
}
......@@ -683,10 +683,10 @@ 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 < B_tray.size(); i++)
for(int i = 0; i < g_tray.size(); i++)
{
std::cout << "index matrix" << i << std::endl;
std::cout << B_tray[i] << std::endl;
std::cout << g_tray[i] << std::endl;
}
// debug
......@@ -696,7 +696,21 @@ void setpointCallback(const Setpoint& newSetpoint)
// std::cout << B.format(OctaveFmt) << std::endl;
// debug of euler method function:
std::vector<VectorXtype> x_out;
VectorXtype x0;
VectorUtype u;
x0 << 0, 0, 0, 0, 0, 0;
u << 0, 0, 0, params.m*params.g*2;
euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x0, u, params, x_out);
for(int i = 0; i < x_out.size(); i++)
{
std::cout << "xout index:" << i << std::endl;
std::cout << x_out[i] << 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