Ejemplo n.º 1
0
void InteractiveResultsTableDelete(InteractiveResultsTable* table)
{
  if(!table)
    return;
  
  MatrixDelete(table->m_results);
  free(table);
  return;
}
Ejemplo n.º 2
0
// 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;
}
Ejemplo n.º 3
0
Archivo: qr.c Proyecto: r-barnes/ocp
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);
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
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);  
}