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;
}
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;
}
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);
}
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;
}