Ejemplo n.º 1
0
void Solver::DiagramQc(){
    double time0, time1;
    time0 = omp_get_wtime();

    #pragma omp parallel
    {
        int id = omp_get_thread_num();
        int threads = omp_get_num_threads();

        for (int n=id; n<Nkhpp; n+=threads) blockskhpp[n]->SetUpMatrices_Qc(t0);
    }
    time1 = omp_get_wtime(); //cout << "Inside Qc. SetUpMatrices needs: " << time1-time0 << " seconds" << endl;

    time0 = omp_get_wtime();
    for (int n=0; n<Nkhpp; n++){

        mat Qc = 0.5*blockskhpp[n]->T * blockskhpp[n]->V * blockskhpp[n]->T2 / blockskhpp[n]->epsilon;

        for (int k1=0; k1<blockskhpp[n]->Nk1; k1++){
            for (int k2=0; k2<blockskhpp[n]->Nk3; k2++){

                int j=blockskhpp[n]->K1(k1,0); int i=blockskhpp[n]->K3(k2,0);
                int a=blockskhpp[n]->K3(k2,1); int b=blockskhpp[n]->K3(k2,2);

                t( Index(a,b,i,j)) -= Qc( k2,k1 );
                t( Index(a,b,j,i)) += Qc( k2,k1 );
                //StoreT(Index(a,b,i,j), -Qc( k2,k1 ));
                //StoreT(Index(a,b,j,i), Qc( k2,k1 ));

            }
        }
    }
    time1 = omp_get_wtime(); //cout << "Inside Qc. Calculate matrices needs: " << time1-time0 << " seconds" << endl;
}
Ejemplo n.º 2
0
Kf::Kf():y_k_k_1(STATES_), y_k_k(STATES_), Y_k_k_1(STATES_, STATES_),
           Y_k_k(STATES_, STATES_), F(STATES_, STATES_), 
           x_k_k(STATES_), x_k_k_1(STATES_), H_enc(STATES_, STATES_),
           H_gps(3, STATES_), Qc(STATES_, STATES_), G(STATES_, STATES_),
           Q(STATES_, STATES_), Y(STATES_, STATES_), y(STATES_)
{
  y_k_k_1 = 0;
  y_k_k = 0;
  Y_k_k_1 = 0;
  Y_k_k = 0;
  y = 0.05;
  Y = 0;
  F = 0;
  F(1, 4) = 1; F(2, 5) = 1; F(3, 6) = 1;
  x_k_k = 0;
  x_k_k_1 = 0;
  H_enc = 0;
  Qc = 0;
  Qc(1, 1) = 0.05; Qc(2, 2) = 0.05; Qc(3, 3) = 0.05;
  Qc(4, 4) = 0.01; Qc(5, 5) = 0.01; Qc(6, 6) = 0.01;
  Qc(7, 7) = 0.005; Qc(8, 8) = 0.005; Qc(9, 9) = 0.005;
  G = 0;
  G(1, 1) = 1; G(2, 2) = 1; G(3, 3) = 1;
  Q = 0;
    
  for (int i = 1; i <= STATES_; i++)
  {
    for (int j = 1; j <= STATES_; j++)
    {
      if (i == j)
      {
        H_enc(i, j) = 1;
        Y_k_k(i, j) = 1;
        Y(i, j) = 0.1;
      }
    }
  }
  
  H_gps = 0;
  H_gps(1,1) = 1;
  H_gps(2,2) = 1;
  H_gps(3,3) = 1;
  
  predict_ = false;
}
Ejemplo n.º 3
0
        double BSplineMotionError<SPLINE_T>::evaluateErrorImplementation()
        {       
            // the error is a scalar: c' Q c, with c the vector valued spline coefficients stacked

            const double* cMat = &((_splineDV->spline()).coefficients()(0,0));
            Eigen::Map<const Eigen::VectorXd> c = Eigen::VectorXd::Map(cMat, _coefficientVectorLength);

            // Q*c :
            // create result container:
            Eigen::VectorXd Qc(_Q.rows());  // number of rows of Q:
            Qc.setZero();
           //  std::cout << Qc->rows() << ":" << Qc->cols() << std::endl;
            _Q.multiply(&Qc, c);
            
            return c.transpose() * (Qc);
        }
Ejemplo n.º 4
0
bool ChIntegrableIIorder::StateSolveA(ChStateDelta& Dvdt,       // result: computed a for a=dv/dt
                                      ChVectorDynamic<>& L,     // result: computed lagrangian multipliers, if any
                                      const ChState& x,         // current state, x
                                      const ChStateDelta& v,    // current state, v
                                      const double T,           // current time T
                                      const double dt,          // timestep (if needed)
                                      bool force_state_scatter  // if false, x,v and T are not scattered to the system
                                      ) {
    if (force_state_scatter)
        StateScatter(x, v, T);

    ChVectorDynamic<> R(GetNcoords_v());
    ChVectorDynamic<> Qc(GetNconstr());
    const double Delta = 1e-6;

    LoadResidual_F(R, 1.0);

    LoadConstraint_C(Qc, -2.0 / (Delta * Delta));

    // numerical differentiation to get the Qc term in constraints
    ChStateDelta dx(v);
    dx *= Delta;
    ChState xdx(x.GetRows(), this);

    StateIncrement(xdx, x, dx);
    StateScatter(xdx, v, T + Delta);
    LoadConstraint_C(Qc, 1.0 / (Delta * Delta));

    StateIncrement(xdx, x, -dx);
    StateScatter(xdx, v, T - Delta);
    LoadConstraint_C(Qc, 1.0 / (Delta * Delta));

    StateScatter(x, v, T);  // back to original state

    bool success = StateSolveCorrection(Dvdt, L, R, Qc, 1.0, 0, 0, x, v, T, false, true);

    return success;
}