void
HHV4Vector::Rotate(const HHV4Vector & q)
{
Double_t ex, ey, ez, en;
Double_t a, b, c, d, px1, py1, pz1, px2, py2, pz2, px3, py3, pz3;
px1 = Px();
py1 = Py();
pz1 = Pz();
ex = q.Px();
ey = q.Py();
ez = q.Pz();
en = sqrt(ex * ex + ey * ey + ez * ez);
ex = ex / en;
ey = ey / en;
ez = ez / en;
// cout << "Rotate e: " << ex << " " << ey << " " << ez << endl;
a = ez;
b = sqrt(1. - a * a);
c = ex / b;
d = -ey / b;
px2 = a * px1 + b * pz1; // rotation around y-axis
py2 = py1;
pz2 = -b * px1 + a * pz1;
// cout << "Rotate p2: " << px2 << " " << py2 << " " << pz2 << endl;
px3 = c * px2 + d * py2; // rotation around z-axis
py3 = -d * px2 + c * py2;
pz3 = pz2;
// cout << "Rotate p3: " << px3 << " " << py3 << " " << pz3 << endl;
SetPxPyPzE(px3, py3, pz3, E());
}
void
HHV4Vector::Boost(Double_t bx, Double_t by, Double_t bz)
{
//Boost this Lorentz vector
//cout << "Boost: beta " << bx << " " << by << " " << bz << " " << bx**2+by**2+bz**2 << endl;
// cout << "Boost: P " << Px()<<" " << Py()<<" " << Pz()<<" " << endl;
Double_t b2 = bx * bx + by * by + bz * bz;
Double_t gamma = 1.0 / sqrt(1.0 - b2);
Double_t bp = bx * Px() + by * Py() + bz * Pz();
Double_t gamma2 = b2 > 0 ? (gamma - 1.0) / b2 : 0.0;
Double_t px, py, pz;
// cout << "Boost: b2,g,bp,g2: " << b2 << " " << gamma << " " << bp << " " << gamma2 << endl;
px = Px() + gamma2 * bp * bx + gamma * bx * E();
py = Py() + gamma2 * bp * by + gamma * by * E();
pz = Pz() + gamma2 * bp * bz + gamma * bz * E();
m_e = gamma * (E() + bp);
m_phi = atan2(py, px);
m_eta = -log(tan(0.5 * acos(pz / sqrt(px * px + py * py + pz * pz))));
// cout << "Boost: E " << E << " px " << px<< " py " << py<< " pz " << pz
// << " Phi " <<Phi<<" Eta " <<Eta << endl;
}
void
PreconditionerBlockMS<space_type>::initAMS( void )
{
M_grad = Grad( _domainSpace=M_Qh, _imageSpace=M_Vh);
// This preconditioner is linked to that backend : the backend will
// automatically use the preconditioner.
auto prec = preconditioner(_pc=pcTypeConvertStrToEnum(soption(M_prefix_11+".pc-type")),
_backend=backend(_name=M_prefix_11),
_prefix=M_prefix_11,
_matrix=M_11
);
prec->setMatrix(M_11);
prec->attachAuxiliarySparseMatrix("G",M_grad.matPtr());
if(boption(M_prefix_11+".useEdge"))
{
LOG(INFO) << "[ AMS ] : using SetConstantEdgeVector \n";
ozz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(1),cst(0),cst(0)));
zoz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(1),cst(0)));
zzo.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(0),cst(1)));
*M_ozz = ozz; M_ozz->close();
*M_zoz = zoz; M_zoz->close();
*M_zzo = zzo; M_zzo->close();
prec->attachAuxiliaryVector("Px",M_ozz);
prec->attachAuxiliaryVector("Py",M_zoz);
prec->attachAuxiliaryVector("Pz",M_zzo);
}
else
{
LOG(INFO) << "[ AMS ] : using SetCoordinates \n";
X.on(_range=elements(M_Vh->mesh()),_expr=Px());
Y.on(_range=elements(M_Vh->mesh()),_expr=Py());
Z.on(_range=elements(M_Vh->mesh()),_expr=Pz());
*M_X = X; M_X->close();
*M_Y = Y; M_Y->close();
*M_Z = Z; M_Z->close();
prec->attachAuxiliaryVector("X",M_X);
prec->attachAuxiliaryVector("Y",M_Y);
prec->attachAuxiliaryVector("Z",M_Z);
}
}
void
HHV4Vector::PrintEPxPyPzM(int pos) const
{
if (pos >= 0) {
PSTools::coutf(4, pos);
std::cout << " ";
}
PSTools::coutf(10, m_name);
PSTools::coutf(4, ID());
PSTools::coutf(9, 2, E());
PSTools::coutf(9, 2, Px());
PSTools::coutf(9, 2, Py());
PSTools::coutf(9, 2, Pz());
PSTools::coutf(9, 2, M());
PSTools::coutf(5, Mother());
for (int i = 0; i < nDaughter(); i++) {
PSTools::coutf(4, Daughter(i));
}
std::cout << std::endl;
// cout << Name << " " << ID << " ";
// Printf(" (EPxPyPzM)= %8.2f %8.2f %8.2f %8.2f %8.2f", E(), Px(), Py(),
// Pz(), M());
}
void FifthCKF::FifthCKFfiltering(double *z, UINT m_length)
{
CMatrix Pplus(4, 4);
Pplus = glb.Pplus;
CMatrix xhat(4, 1);
xhat = glb.xhat;
StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1));
X.Add(fifthckf);
CMatrix Shat(dim, dim);
CMatrix rjpoint1(dim, 1);
CMatrix rjpoint2(dim, cpoints);
CMatrix rjpoint3(dim, hcpoints - cpoints - 1);
CMatrix Xminus1(dim, 1);
CMatrix Xminus2(dim, cpoints);
CMatrix Repmat2(dim, cpoints);
CMatrix Xminus3(dim, hcpoints - cpoints - 1);
CMatrix Repmat3(dim, hcpoints - cpoints - 1);
CMatrix jtemp(dim, 1);
CMatrix Zhat1(1, 1);
CMatrix Zhat2(1, cpoints);
CMatrix RepmatZ2(1, cpoints);
CMatrix Zhat3(1, hcpoints - cpoints - 1);
CMatrix RepmatZ3(1, hcpoints - cpoints - 1);
CMatrix W(dim, 1);
CMatrix Pz(1, 1);
CMatrix Xtemp(dim, 1);
CMatrix Pxz(4, 1);
int num = 1;//第0个未知存放的是占位符0，并不是真正的测量值，因此应当从下标1开始取测量值
for (UINT count = TimeInterval; count <= m_length; count += TimeInterval)
{
//Time Update
//Evaluate the Cholesky factor
Shat = Pplus.Cholesky();
//Evaluate the cubature points and the propagated cubature points//Estimate the predicted state
CMatrix CX(4, 1);
for (int i = 1; i <= hcpoints; ++i)
{
for (int j = 1; j <= dim; ++j)
{
jtemp(j, 1) = glb.kesi_FifthCKF(j, i);
}
jtemp = Shat * jtemp + xhat;
Xtemp = glb.fai * jtemp;
if ( 1 == i)
{
for (int j = 1; j <= dim; ++j)
{
Xminus1(j, 1) = Xtemp(j, 1);
}
CX = CX + c5_w2 * Xtemp;
}
else if (i > 1 && i <= cpoints + 1)//此编程风格会提高代码的冗余度，但提高了程序的运行效率，下同
{
for (int j = 1; j <= dim; ++j)
{
Xminus2(j, i - 1) = Xtemp(j, 1);
}
CX = CX + c5_w1 * Xtemp;
}
else
{
for (int j = 1; j <= dim; ++j)
{
Xminus3(j, i - cpoints - 1) = Xtemp(j, 1);
}
CX = CX + c5_w4 * Xtemp;
}
}
//xhat = CX;
//Estimate the predicted error covariance
for (int i = 1; i <= cpoints; ++i)
{
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
Repmat2(j, i) = CX(j, 1);
}
}
for (int i = 1; i <= hcpoints - cpoints - 1; ++i)
{
#pragma omp parallel for
for(int j = 1; j <= dim; ++j)
{
Repmat3(j, i) = CX(j, 1);
}
}
Pplus = c5_w2 * ((Xminus1 - CX) * (~(Xminus1 - CX))) +
c5_w1 * (Xminus2 - Repmat2) * (~(Xminus2 - Repmat2)) +
c5_w4 * (Xminus3 - Repmat3) * (~(Xminus3 - Repmat3)) + glb.gama * glb.Im * (~glb.gama);
//Measurement Update
//Evaluate the Cholesky factor
Shat = Pplus.Cholesky();
//Evaluate the cubature points and the propagated cubature points//Estimate the predicted measurement
CMatrix Z(1, 1);
for (int i = 1; i <= hcpoints; ++i)
{
for (int j = 1; j <= dim; ++j)
{
jtemp(j, 1) = glb.kesi_FifthCKF(j, i);
}
jtemp = Shat * jtemp + CX;
if (1 == i)
{
Zhat1(1, 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w2 * Zhat1(1, 1);
#pragma omp parallel for
for (int j = 1; j<= dim; ++j)
{
rjpoint1(j, 1) = jtemp(j, 1);
}
}
else if (i > 1 && i <= cpoints + 1)
{
Zhat2(1, i - 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w1 * Zhat2(1, i - 1);
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
rjpoint2(j, i - 1) = jtemp(j, 1);
}
}
else
{
Zhat3(1, i - cpoints - 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w4 * Zhat3(1, i - cpoints - 1);
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
rjpoint3(j, i - cpoints - 1) = jtemp(j, 1);
}
}
}
//Estimate the innovation covariance matrix
Pz(1, 1) = Rn;//For saving memory
#pragma omp parallel for
for (int i = 1; i <= cpoints; ++i)
{
RepmatZ2(1, i) = Z(1, 1);
}
#pragma omp parallel for
for (int i = 1; i <= hcpoints - cpoints - 1; ++i)
{
RepmatZ3(1, i) = Z(1, 1);
}
Pz = c5_w2 * (Zhat1 - Z) * (~(Zhat1 - Z)) +
c5_w1 * (Zhat2 - RepmatZ2)* (~(Zhat2 - RepmatZ2)) +
c5_w4 * (Zhat3 - RepmatZ3) * (~(Zhat3 - RepmatZ3)) + Pz;
//Estimate the cross-covariance matrix
Pxz = c5_w2 * (rjpoint1 - CX) * (~(Zhat1 - Z)) +
c5_w1 *(rjpoint2 - Repmat2) * (~(Zhat2 - RepmatZ2)) +
c5_w4 * (rjpoint3 - Repmat3) * (~(Zhat3 - RepmatZ3));
//Estimate the Kalman gain
W = ((double)1 / Pz(1, 1)) * Pxz;
//Estimate the updated state
//Znum(1, 1) = z[num];
xhat = CX + W * (z[num] - Z(1, 1));
++num;
Pplus = Pplus - Pz(1, 1) * W * (~W);
StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1));
X.Add(fifthckf);
}
}
Vector3< Momentum > PseudoJet::Spatial() const
{
return {Px(), Py(), Pz()};
}
boca::LorentzVector< Momentum > PseudoJet::LorentzVector() const
{
return {Px(), Py(), Pz(), Energy()};
}
PreconditionerBlockMS<space_type>::PreconditionerBlockMS(space_ptrtype Xh, // (u)x(p)
ModelProperties model, // model
std::string const& p, // prefix
sparse_matrix_ptrtype AA ) // The matrix
:
M_backend(backend()), // the backend associated to the PC
M_Xh( Xh ),
M_Vh( Xh->template functionSpace<0>() ), // Potential
M_Qh( Xh->template functionSpace<1>() ), // Lagrange
M_Vh_indices( M_Vh->nLocalDofWithGhost() ),
M_Qh_indices( M_Qh->nLocalDofWithGhost() ),
M_uin( M_backend->newVector( M_Vh ) ),
M_uout( M_backend->newVector( M_Vh ) ),
M_pin( M_backend->newVector( M_Qh ) ),
M_pout( M_backend->newVector( M_Qh ) ),
U( M_Xh, "U" ),
M_mass(M_backend->newMatrix(M_Vh,M_Vh)),
M_L(M_backend->newMatrix(M_Qh,M_Qh)),
M_er( 1. ),
M_model( model ),
M_prefix( p ),
M_prefix_11( p+".11" ),
M_prefix_22( p+".22" ),
u(M_Vh, "u"),
ozz(M_Vh, "ozz"),
zoz(M_Vh, "zoz"),
zzo(M_Vh, "zzo"),
M_ozz(M_backend->newVector( M_Vh )),
M_zoz(M_backend->newVector( M_Vh )),
M_zzo(M_backend->newVector( M_Vh )),
X(M_Qh, "X"),
Y(M_Qh, "Y"),
Z(M_Qh, "Z"),
M_X(M_backend->newVector( M_Qh )),
M_Y(M_backend->newVector( M_Qh )),
M_Z(M_backend->newVector( M_Qh )),
phi(M_Qh, "phi")
{
tic();
LOG(INFO) << "[PreconditionerBlockMS] setup starts";
this->setMatrix( AA );
this->setName(M_prefix);
/* Indices are need to extract sub matrix */
std::iota( M_Vh_indices.begin(), M_Vh_indices.end(), 0 );
std::iota( M_Qh_indices.begin(), M_Qh_indices.end(), M_Vh->nLocalDofWithGhost() );
M_11 = AA->createSubMatrix( M_Vh_indices, M_Vh_indices, true, true);
/* Boundary conditions */
BoundaryConditions M_bc = M_model.boundaryConditions();
map_vector_field<FEELPP_DIM,1,2> m_dirichlet_u { M_bc.getVectorFields<FEELPP_DIM> ( "u", "Dirichlet" ) };
map_scalar_field<2> m_dirichlet_p { M_bc.getScalarFields<2> ( "phi", "Dirichlet" ) };
/* Compute the mass matrix (needed in first block, constant) */
auto f2A = form2(_test=M_Vh, _trial=M_Vh, _matrix=M_mass);
auto f1A = form1(_test=M_Vh);
f2A = integrate(_range=elements(M_Vh->mesh()), _expr=inner(idt(u),id(u))); // M
for(auto const & it : m_dirichlet_u )
{
LOG(INFO) << "Applying " << it.second << " on " << it.first << " for "<<M_prefix_11<<"\n";
f2A += on(_range=markedfaces(M_Vh->mesh(),it.first), _expr=it.second,_rhs=f1A, _element=u, _type="elimination_symmetric");
}
/* Compute the L (= er * grad grad) matrix (the second block) */
auto f2L = form2(_test=M_Qh,_trial=M_Qh, _matrix=M_L);
for(auto it : M_model.materials() )
{
f2L += integrate(_range=markedelements(M_Qh->mesh(),marker(it)), _expr=M_er*inner(gradt(phi), grad(phi)));
}
auto f1LQ = form1(_test=M_Qh);
for(auto const & it : m_dirichlet_p)
{
LOG(INFO) << "Applying " << it.second << " on " << it.first << " for "<<M_prefix_22<<"\n";
f2L += on(_range=markedfaces(M_Qh->mesh(),it.first),_element=phi, _expr=it.second, _rhs=f1LQ, _type="elimination_symmetric");
}
if(soption(_name="pc-type", _prefix=M_prefix_11) == "ams")
#if FEELPP_DIM == 3
{
M_grad = Grad( _domainSpace=M_Qh, _imageSpace=M_Vh);
// This preconditioner is linked to that backend : the backend will
// automatically use the preconditioner.
auto prec = preconditioner(_pc=pcTypeConvertStrToEnum(soption(M_prefix_11+".pc-type")),
_backend=backend(_name=M_prefix_11),
_prefix=M_prefix_11,
_matrix=M_11
);
prec->setMatrix(M_11);
prec->attachAuxiliarySparseMatrix("G",M_grad.matPtr());
if(boption(M_prefix_11+".useEdge"))
{
LOG(INFO) << "[ AMS ] : using SetConstantEdgeVector \n";
ozz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(1),cst(0),cst(0)));
zoz.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(1),cst(0)));
zzo.on(_range=elements(M_Vh->mesh()),_expr=vec(cst(0),cst(0),cst(1)));
*M_ozz = ozz; M_ozz->close();
*M_zoz = zoz; M_zoz->close();
*M_zzo = zzo; M_zzo->close();
prec->attachAuxiliaryVector("Px",M_ozz);
prec->attachAuxiliaryVector("Py",M_zoz);
prec->attachAuxiliaryVector("Pz",M_zzo);
}
else
{
LOG(INFO) << "[ AMS ] : using SetCoordinates \n";
X.on(_range=elements(M_Vh->mesh()),_expr=Px());
Y.on(_range=elements(M_Vh->mesh()),_expr=Py());
Z.on(_range=elements(M_Vh->mesh()),_expr=Pz());
*M_X = X; M_X->close();
*M_Y = Y; M_Y->close();
*M_Z = Z; M_Z->close();
prec->attachAuxiliaryVector("X",M_X);
prec->attachAuxiliaryVector("Y",M_Y);
prec->attachAuxiliaryVector("Z",M_Z);
}
}
#else
std::cerr << "ams preconditioner is not interfaced in two dimensions\n";
#endif
toc( "[PreconditionerBlockMS] setup done ", FLAGS_v > 0 );
}
