void InteractiveResultsTableDelete(InteractiveResultsTable* table)
{
if(!table)
return;
MatrixDelete(table->m_results);
free(table);
return;
}
// form H = (1/h) W (x) I - I (x) J
// get the L, U, p factors of H
int _formIRKmatrixZ(RKOCP r, Vector y, Vector u, Vector p, double t, double h, Matrix L, Matrix U, int *P) {
int n = r->n_states+1;
int s = r->rk_stages;
double **W = r->rk_w->e;
int err = 0;
int i, j, k, l;
Matrix H = MatrixNew(n*s, n*s);
Matrix J = MatrixNew(n, n);
// get Fy, Fu, Fp, Ly, Lu, Lp
if (r->ocp->Ddifferential_equations == NULL) {
OCPFDfL(r->ocp, y, u, p, t, r->Fy, r->Fu, r->Fp, r->Ly, r->Lu, r->Lp);
} else {
r->ocp->Ddifferential_equations(y, u, p, t, r->Fy, r->Fu, r->Fp, r->Ly, r->Lu, r->Lp);
}
for (i = 0; i < r->n_states; i ++) {
for (j = 0; j < r->n_states; j++) {
J->e[i][j] = r->Fy->e[i][j];
}
J->e[r->n_states][i] = r->Ly->e[i];
}
// H = (1/h) W (x) I - I (x) J
for (i = 0; i < s; i++) {
for (j = 0; j < s; j++) {
double wij_h = W[i][j] / h;
for (k = 0; k < n; k++) {
H->e[i*n + k][j*n + k] += wij_h;
}
}
for (k = 0; k < n; k++) {
for (l = 0; l < n; l++) {
H->e[i*n + k][i*n + l] -= J->e[k][l];
}
}
}
err = LUFactor(H, L, U, P);
MatrixDelete(J);
MatrixDelete(H);
return err;
}
void _householder(double **Q, double **R, double **B, int n, int m) {
int i, j, k;
Vector Beta = VectorNew(m);
Vector W = VectorNew(n);
Matrix V = MatrixNew(n, n);
double *beta = Beta->e;
// Q = I
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
if (i == j) {
Q[i][j] = 1.0;
} else {
Q[i][j] = 0.0;
}
}
}
// R = B
for (i = 0; i < n; i++) {
double *Ri = R[i];
double *Bi = B[i];
for (k = 0; k < m; k++) {
Ri[k] = Bi[k];
}
}
// find the Householder vectors for each column of R
for (k = 0; k < m; k++) {
double xi;
double norm_x = 0.0;
double *w = W->e;
double *v = V->e[k];
for (i = k; i < n; i++) {
xi = R[i][k];
norm_x += xi * xi;
v[i] = xi;
}
norm_x = sqrt(norm_x);
double x1 = v[k];
double sign_x1 = sign(x1);
double gamma = -1.0 * sign_x1 * norm_x;
double v_dot_v = 2.0 * norm_x * (norm_x + sign_x1 * x1);
if (v_dot_v < DBL_EPSILON) {
for (i = k; i < m; i++) {
R[i][i] = 0.0;
}
VectorDelete(Beta);
VectorDelete(W);
MatrixDelete(V);
return;
}
v[k] -= gamma;
beta[k] = -2.0 / v_dot_v;
// w(k:m) = R(k:n, k:m)^T * v(k:n)
for (i = k; i < m; i++) {
double s = 0.0;
for (j = k; j < n; j++) { // FIX: make this row-wise
s += R[j][i] * v[j];
}
w[i] = s;
}
// R(k:n, k:m) += beta * v(k:n) * w(k:m)^T
//a[k : n, k : m] = a[k : n, k : m] + beta * (v * wT)
for (i = k; i < n; i++) {
for (j = k; j < m; j++) {
R[i][j] += beta[k] * v[i] * w[j];
}
}
}
for (k = m-1; k >= 0; k--) {
double *v = V->e[k];
double *u = W->e;
//uT = v.transpose() * Q[k : n, k : n]
for (i = k; i < n; i++) {
double s = 0.0;
for (j = k; j < n; j++) {
s += Q[j][i] * v[j];
}
u[i] = s;
}
//Q[k : n, k : n] = Q[k : n, k : n] + Beta[k] * (v * uT)
for (i = k; i < n; i++) {
for (j = k; j < n; j++) {
Q[i][j] += beta[k] * v[i] * u[j];
}
}
}
VectorDelete(Beta);
VectorDelete(W);
MatrixDelete(V);
}
int main(){
ulong i,j;
LinearSystem *system;
Matrix * matrix = MatrixInit(3,4);
/*
for(i=0; i < matrix->m_rows; i++)
for(j=0; j < matrix->m_columns; j++)
matrix->m_data[i][j] = (3.14 * (i+(j+1)) * 2) / 10;
*/
// ONLY TEST ---------
matrix->m_data[0][0] = 1;
matrix->m_data[0][1] = 1;
matrix->m_data[0][2] = 2;
matrix->m_data[0][3] = 9;
matrix->m_data[1][0] = 0;
matrix->m_data[1][1] = 2;
matrix->m_data[1][2] = -7;
matrix->m_data[1][3] = -17;
matrix->m_data[2][0] = 3;
matrix->m_data[2][1] = 6;
matrix->m_data[2][2] = -5;
matrix->m_data[2][3] = 0;
// -----------------------
MatrixShow(matrix);
system = LinearSystemInit(matrix, TRUE);
MatrixShow(system->m_systemMatrix);
MatrixDelete(matrix);
//LinearSystemSetIndependentTermsVector(system, 0.1,0.2,0.3,0.5,0.4,0.03);
MatrixShow(system->m_systemMatrix);
LinearSystemGaussJordan(system);
MatrixShow(system->m_solutionMatrix);
LinearSystemDelete(system);
Polynomial *pol = PolynomialInit(2);
PolynomialSetConstants(pol, 1.0, 0.0,-3.0);
OrderedPair *pair = (OrderedPair*) malloc(sizeof(OrderedPair));
pair->m_x = 1;
pair->m_y = 2;
printf("Intervals %lf %lf\n", pair->m_x, pair->m_y);
PolynomialResultsTable *table = PolynomialRootBissection(pol,pair,10,0.01);
//printf("%lu %lf\n",table->m_results->m_iterator, table->m_results[1].m_data[1]);
if (table){
puts("\n Bissecao \n\n");
PolynomialResultsTableShow(table);
PolynomialResultsTableDelete(table);
}
Polynomial *pol2 = PolynomialInit(3);
PolynomialSetConstants(pol2,1.0,0.0,(-9.0),3.0);
PolynomialShow(pol2);
pair->m_x = 0;
pair->m_y = 1;
table = PolynomialRootSecant(pol2, pair , 10, 0.0005);
puts("\n SECANTE \n\n");
PolynomialResultsTableShow(table);
printf("\nRoot founded is: %lf\n", *table->m_root);
PolynomialResultsTableDelete(table);
PolynomialShow(pol);
PolynomialDelete(pol);
free(pair);
PolynomialDelete(pol2);
return 0;
}
void _RKOCPDfhg_implicitZ(Vector nlp_x, Vector nlp_df, Matrix nlp_dh, Matrix nlp_dg) {
/*
TODO: Instead of copying data to ys and us
set ys->r = n_states+1, ys->e = state data
set us->r = n_controls, us->e = control data
*/
// nlp_x: vec[P, Y(0), U(0, :), U(1, :), ..., U(n_nodes-1, :)]
// Yx: state+cost sensitivity with respect to nlp_x
RKOCP r = thisRKOCP;
int i_node, i, j;
double t, ti, h;
int m = r->nlp_m;
int p = r->nlp_p;
int n_nodes = r->n_nodes;
int n_parameters = r->n_parameters;
int n_states = r->n_states;
int n_controls = r->n_controls;
int n_inequality = r->ocp->n_inequality;
int n_initial = r->ocp->n_initial;
int n_terminal = r->ocp->n_terminal;
int rk_stages = r->rk_stages;
double *rk_c = r->rk_c->e;
Vector T = r->T;
Matrix Yw = r->Yw;
Matrix Yx = r->Yx;
Matrix *Ydata = r->Ydata;
Matrix *Udata = r->Udata;
Matrix *Yx_stage = r->Yx_stage;
Matrix *Zx_stage = r->Kx_stage;
Vector ys = r->yc;
Vector us = r->uc;
Vector ps = r->pc;
// FIX: preallocate the following data
Matrix Yx_dot = MatrixNew(n_states+1, r->nlp_n);
Matrix res = MatrixNew(rk_stages*(n_states+1), r->nlp_n);
Matrix dZx = MatrixNew(rk_stages*(n_states+1), r->nlp_n);
Matrix Kx = MatrixNew(n_states+1, r->nlp_n);
VectorSetAllTo(nlp_df, 0.0);
if (m > 0) {
MatrixSetAllTo(nlp_dh, 0.0);
}
if (p > 0) {
MatrixSetAllTo(nlp_dg, 0.0);
}
if (r->integration_fatal_error == YES) {
return;
}
if (n_controls > 0) {
us = VectorNew(n_controls);
}
// parameters: ps
if (n_parameters > 0) {
ps = VectorNew(n_parameters);
for (i = 0; i < n_parameters; i++)
ps->e[i] = nlp_x->e[i];
}
// sensitivity: Yx(t_initial)
MatrixSetAllTo(Yx, 0.0);
for (i = 0; i < n_states; i++) {
Yx->e[i][i+n_parameters] = 1.0;
}
for (i_node = 0; i_node < n_nodes-1; i_node++) {
ti = T->e[i_node];
h = T->e[i_node+1] - ti;
t = ti;
// states: ys(t)
for (i = 0; i < n_states+1; i++)
ys->e[i] = Yw->e[i_node][i];
// controls: us(t)
for (i = 0; i < n_controls; i++)
us->e[i] = nlp_x->e[n_parameters + n_states + i_node*n_controls + i];
// dGamma -> nlp_dh
if ((i_node == 0) && (n_initial > 0)) {
_setDhGamma(r, ys, ps, nlp_dh);
}
// dg(t) -> nlp_dg
if (n_inequality > 0) {
_setDg(r, Yx, ys, us, ps, t, i_node, nlp_dg);
}
/*
Integrate the sensitivity equations using an implicit Runge-Kuta method
Yx_dot = Fy*Yx + Fu*ux + Fp*px
Fy = [fy; Ly], Fu = [fu; Lu], Fp = [fp; Lp]
Yx : (n_states+1)-by-nlp_n matrix
Yx_dot : (n_states+1)-by-nlp_n matrix
Ux : n_controls-by-nlp_n matrix
Ux : n_parameters-by-nlp_n matrix
Algorithm:
recover the LUp factors of DPhi = (1/h) w (x) I - I (x) J
estimate Zx[i], i = 1, 2, .. stages
for iter = 0, 1, ..., MAX_NEWTON_ITER
for i = 1,2,...,stages
t_i = t + c_{i}*h
Yx_i = Yx + Zx[i]
Yx_dot_i = (1/h)*sum_{j=1,s} w_{i,j} * Zx[j]
get Ux_i
get Px_i
compute Phi_i = Yx_dot_i - (Fy*Yx_i + Fu*Ux_i + Fp*Px_i)
end
solve DPhi*dZx = -Phi
Zx += dZx
Yx_i = Yx + Zx_i, i=1,2,...,s
check for convergence, i.e, ||dZx|| <= 0.01*tol
end
update Yx = Yx_s (Assuming that the Runge-Kutta method has c[s] = 1
Since these equations are linear we should expect convergence
in a few iterations.
*/
int iter, MAX_NEWTON = 15;
// recover L, U, P
Matrix L = r->Lnode[i_node];
Matrix U = r->Unode[i_node];
int *P = r->pLUnode[i_node];
// set the column index for the sensitivity unknowns at this node
int c0, c1;
r->column_index = 0;
switch (r->c_type) {
case CONSTANT:
r->column_index = n_parameters + n_states + (i_node + 1) * n_controls;
break;
case LINEAR:
r->column_index = n_parameters + n_states + (i_node + 2) * n_controls;
break;
case CUBIC:
c0 = (i_node + 6) * n_controls;
c1 = n_nodes * n_controls;
c0 = c0 < c1 ? c0 : c1;
r->column_index = n_parameters + n_states + c0;
break;
}
// get Zx_stage estimate
_setZx_stage(r, i_node, Zx_stage);
for (iter = 0; iter < MAX_NEWTON; iter++) {
_setYx_stage_implicit(r, Yx, Zx_stage, Yx_stage);
for (i = 0; i < rk_stages; i++) {
ti = t + h*rk_c[i];
_setYx_dot_stage_implicit(r, Zx_stage, h, i, Yx_dot);
for (j = 0; j < n_controls; j++) us->e[j] = Udata[i_node]->e[i][j];
for (j = 0; j < n_states+1; j++) ys->e[j] = Ydata[i_node]->e[i][j];
_setResidual_implicit(r, i_node, i, Yx_dot, Yx_stage[i], ys, us, ps, t, Kx, res);
}
LUSolveM(L, U, P, res, dZx);
// update Zx_stage
_updateZx_stage(r, Zx_stage, dZx);
// update Yx_stage
_setYx_stage_implicit(r, Yx, Zx_stage, Yx_stage);
// check for convergence
double normdZx = MatrixNorm(dZx);
if (normdZx <= 0.01*r->tolerance) {
break;
}
}
// set Yx = Yx_stage[rk_stage-1]
for (i = 0; i < n_states+1; i++) {
//for (j = 0; j < r->nlp_n; j++) { // FIX: make this i_node dependent
for (j = 0; j < r->column_index; j++) {
Yx->e[i][j] = Yx_stage[rk_stages-1]->e[i][j];
}
}
}
i_node = n_nodes-1;
t = T->e[i_node];
// states: ys(t)
for (i = 0; i < n_states+1; i++)
ys->e[i] = Yw->e[i_node][i];
// controls: us(t)
for (i = 0; i < n_controls; i++)
us->e[i] = Udata[n_nodes-2]->e[rk_stages-1][i];
// dg(t_{i_node}) -> nlp_dg
if (n_inequality > 0) {
_setDg(r, Yx, ys, us, ps, t, i_node, nlp_dg);
}
// Terminal conditions Psi and penalty phi
if (r->ocp->Dterminal_constraints == NULL) {
OCPFDphiPsi(r->ocp, ys, ps, r->phiy, r->phip, r->Psiy, r->Psip);
} else {
r->ocp->Dterminal_constraints(ys, ps, r->phiy, r->phip, r->Psiy, r->Psip);
}
// dPsi -> nlp_dh
if (n_terminal > 0) {
_setDhPsi(r, Yx, nlp_dh);
}
// df = integral of L
for (i = 0; i < r->nlp_n; i++)
nlp_df->e[i] = Yx->e[n_states][i];
// df += dphi
_addDphi(r, Yx, nlp_df);
MatrixDelete(Yx_dot);
MatrixDelete(res);
MatrixDelete(dZx);
MatrixDelete(Kx);
}
