Matrix<double> BoundingLayer::arrange_Jacobian(const Vector<double>& derivatives) const
{
const size_t bounding_neurons_number = get_bounding_neurons_number();
// Control sentence (if debug)
#ifdef __OPENNN_DEBUG__
const size_t derivatives_size = derivatives.size();
if(derivatives_size != bounding_neurons_number)
{
std::ostringstream buffer;
buffer << "OpenNN Exception: BoundingLayer class.\n"
<< "Matrix<double> arrange_Jacobian(const Vector<double>&) method.\n"
<< "Size of derivatives must be equal to number of bounding neurons.\n";
throw std::logic_error(buffer.str());
}
#endif
Matrix<double> Jacobian(bounding_neurons_number, bounding_neurons_number, 0.0);
Jacobian.set_diagonal(derivatives);
return(Jacobian);
}
/**************************************************
* method SpuriousAttractorIndex
*
* @return a number indicating how far we are from a
* spurious attractor
*
****************************************************/
float Reaching::SpuriousAttractorIndex()
{
joint_vec_t& da,tmp14,tmp24;
cart_vec_t& dx;
CMatrix4_t jac; //jacobian matrix
cart_vec_t& wxm1,tmp13;
float dalength;
v_sub(target,pos_cart,dx);
v4_sub(tar_angle,pos_angle,da);
Jacobian(pos_angle,jac);
inverse_diag3(weight_cart,wxm1);
m3_diag_v_multiply(wxm1,dx,tmp13);
m3_4_t_v_multiply(jac,tmp13,tmp14);
m4_diag_v_multiply(weight_angle,tmp14,tmp24);
v4_add(da,tmp24,tmp14);
dalength = v4_length(da);
if (dalength < 0.00001){
return 2;
}
else{
return (v4_length(tmp14))/dalength;
}
}
double ElementTransformation::EvalWeight()
{
MFEM_ASSERT((EvalState & WEIGHT_MASK) == 0, "");
Jacobian();
EvalState |= WEIGHT_MASK;
return (Wght = (dFdx.Width() == 0) ? 1.0 : dFdx.Weight());
}
int
AC3D8HexWithSensitivity::computeDiff(void)
{
if (L == 0 || detJ == 0) {
L = new Matrix*[numGP];
detJ = new double[numGP];
if (L == 0 || detJ == 0) {
opserr << "AC3D8HexWithSensitivity::computeDiff - out of memory!\n";
return -3;
}
Matrix Jacobian(3,3);
this->computeH();
Matrix NC = getNodalCoords();
for(int i = 0; i < numGP; i++) {
L[i] = new Matrix(3, nodes_in_elem);
if(L[i] == 0) {
opserr << "AC3D8HexWithSensitivity::computDiff() - out of memory!\n";
return -3;
}
Matrix &dh = *DH[i];
Jacobian = dh*NC;
detJ[i] = Jacobian_det(Jacobian);
// compute [Jac]^-1*[DH]
Jacobian.Solve(dh, *L[i]);
}
}
return 0;
}
RVector calcGCells( const std::vector< RVector3 > & pos, const Mesh & mesh, const RVector & model, uint nInt ){
/*! Ensure neighbourInfos() */
RMatrix Jacobian( pos.size(), mesh.cellCount() );
Jacobian *= 0.;
for ( uint i = 0; i < pos.size(); i ++ ){
for ( std::vector< Cell * >::const_iterator it = mesh.cells().begin(); it != mesh.cells().end(); it ++ ){
Cell *c = *it;
double Z = 0.;
if ( nInt == 0 ){
for ( uint j = 0; j < c->nodeCount(); j ++ ){
// negative Z because all cells are numbered counterclockwise
Z -= 2.0 * lineIntegraldGdz( c->node( j ).pos() - pos[ i ], c->node( (j+1)%c->nodeCount() ).pos() - pos[ i ] );
}
} else {
for ( uint j = 0; j < IntegrationRules::instance().triAbscissa( nInt ).size(); j ++ ){
Z += IntegrationRules::instance().triWeights( nInt )[ j ] *
f_gz( c->shape().xyz( IntegrationRules::instance().triAbscissa( nInt )[ j ] ), pos[ i ] );
}
}
Jacobian[ i ][ c->id() ] = -Z;
}
}
return Jacobian * model * 6.67384e-11 * 1e5;
}
Matrix<double> ProbabilisticLayer::calculate_no_probabilistic_Jacobian(const Vector<double>&) const
{
Matrix<double> Jacobian(probabilistic_neurons_number, probabilistic_neurons_number);
Jacobian.initialize_identity();
return(Jacobian);
}
const DenseMatrix &ElementTransformation::EvalAdjugateJ()
{
MFEM_ASSERT((EvalState & ADJUGATE_MASK) == 0, "");
Jacobian();
adjJ.SetSize(dFdx.Width(), dFdx.Height());
if (dFdx.Width() > 0) { CalcAdjugate(dFdx, adjJ); }
EvalState |= ADJUGATE_MASK;
return adjJ;
}
Matrix<double> ScalingLayer::arrange_Jacobian(const Vector<double>& derivatives) const
{
const size_t scaling_neurons_number = get_scaling_neurons_number();
Matrix<double> Jacobian(scaling_neurons_number, scaling_neurons_number, 0.0);
Jacobian.set_diagonal(derivatives);
return(Jacobian);
}
const DenseMatrix &ElementTransformation::EvalInverseJ()
{
// TODO: compute as invJ = / adjJ/Weight, if J is square,
// \ adjJ/Weight^2, otherwise.
MFEM_ASSERT((EvalState & INVERSE_MASK) == 0, "");
Jacobian();
invJ.SetSize(dFdx.Width(), dFdx.Height());
if (dFdx.Width() > 0) { CalcInverse(dFdx, invJ); }
EvalState |= INVERSE_MASK;
return invJ;
}
hMatrix Jacobian_hMatrix(double *theta, double *alpha, double *a, double *d) {
hMatrix T01(4,4), T02(4,4), T03(4,4), T04(4,4), T05(4,4), T06(4,4), T07(4,4);
T01 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 1);
T02 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 2);
T03 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 3);
T04 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 4);
T05 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 5);
T06 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 6);
T07 = T_hMatrix(&theta[0], &alpha[0], &a[0], &d[0], 7);
double k[3] = {0,0,1};
double z1[3] = { T01.element(0,2),T01.element(1,2), T01.element(2,2)};
double z2[3] = { T02.element(0,2),T02.element(1,2), T02.element(2,2)};
double z3[3] = { T03.element(0,2),T03.element(1,2), T03.element(2,2)};
double z4[3] = { T04.element(0,2),T04.element(1,2), T04.element(2,2)};
double z5[3] = { T05.element(0,2),T05.element(1,2), T05.element(2,2)};
double z6[3] = { T06.element(0,2),T06.element(1,2), T06.element(2,2)};
double o1[3] = {T01.element(0,3), T01.element(1,3), T01.element(2,3)};
double o2[3] = {T02.element(0,3), T02.element(1,3), T02.element(2,3)};
double o3[3] = {T03.element(0,3), T03.element(1,3), T03.element(2,3)};
double o4[3] = {T04.element(0,3), T04.element(1,3), T04.element(2,3)};
double o5[3] = {T05.element(0,3), T05.element(1,3), T05.element(2,3)};
double o6[3] = {T06.element(0,3), T06.element(1,3), T06.element(2,3)};
double o7[3] = {T07.element(0,3), T07.element(1,3), T07.element(2,3)};
double O1[3] ={o7[0],o7[1],o7[2]};
double O2[3] ={o7[0]-o1[0],o7[1]-o1[1],o7[2]-o1[2]};
double O3[3] ={o7[0]-o2[0],o7[1]-o2[1],o7[2]-o2[2]};
double O4[3] ={o7[0]-o3[0],o7[1]-o3[1],o7[2]-o3[2]};
double O5[3] ={o7[0]-o4[0],o7[1]-o4[1],o7[2]-o4[2]};
double O6[3] ={o7[0]-o5[0],o7[1]-o5[1],o7[2]-o5[2]};
double O7[3] ={o7[0]-o6[0],o7[1]-o6[1],o7[2]-o6[2]};
hMatrix c1(1,3),c2(1,3),c3(1,3),c4(1,3),c5(1,3),c6(1,3),c7(1,3);
c1 = cross(&k[0],&O1[0]);
c2 = cross(&z1[0],&O2[0]);
c3 = cross(&z2[0],&O3[0]);
c4 = cross(&z3[0],&O4[0]);
c5 = cross(&z4[0],&O5[0]);
c6 = cross(&z5[0],&O6[0]);
c7 = cross(&z6[0],&O7[0]);
double J[42] = { c1.element(0,0), c2.element(0,0), c3.element(0,0), c4.element(0,0), c5.element(0,0), c6.element(0,0), c7.element(0,0),
c1.element(0,1), c2.element(0,1), c3.element(0,1), c4.element(0,1), c5.element(0,1), c6.element(0,1), c7.element(0,1),
c1.element(0,2), c2.element(0,2), c3.element(0,2), c4.element(0,2), c5.element(0,2), c6.element(0,2), c7.element(0,2),
k[0],z1[0],z2[0],z3[0],z4[0],z5[0],z6[0],
k[1],z1[1],z2[1],z3[1],z4[1],z5[1],z6[1],
k[2],z1[2],z2[2],z3[2],z4[2],z5[2],z6[2]};
hMatrix Jacobian(6,7);
Jacobian.SET(6,7,&J[0]);
return Jacobian;
}
/**
* @param position the new position in rad Robota frame of reference
* @param speed the new joint angle speed
* @return 1 if the new position is within the workspace boundaries
* and 0 otherwise (in that case nothing happens)
*
***************************************************/
int Reaching::SetActualRobPosAndSpeed(joint_vec_t& position, joint_vec_t& speed){
CMatrix4_t jac;
if(SetActualRobPosition(position)){
v4_copy(speed,v_angle);
Jacobian(pos_angle,jac);
m3_4_v_multiply(jac,speed,v_cart);
return 1;
}
else{
return 0;
}
}
int IsoparametricTransformation::TransformBack(const Vector &pt,
IntegrationPoint &ip)
{
const int max_iter = 16;
const double ref_tol = 1e-15;
const double phys_tol = 1e-15*pt.Normlinf();
const int dim = FElem->GetDim();
const int sdim = PointMat.Height();
const int geom = FElem->GetGeomType();
IntegrationPoint xip, prev_xip;
double xd[3], yd[3], dxd[3], Jid[9];
Vector x(xd, dim), y(yd, sdim), dx(dxd, dim);
DenseMatrix Jinv(Jid, dim, sdim);
bool hit_bdr = false, prev_hit_bdr;
// Use the center of the element as initial guess
xip = Geometries.GetCenter(geom);
xip.Get(xd, dim); // xip -> x
for (int it = 0; it < max_iter; it++)
{
// Newton iteration: x := x + J(x)^{-1} [pt-F(x)]
// or when dim != sdim: x := x + [J^t.J]^{-1}.J^t [pt-F(x)]
Transform(xip, y);
subtract(pt, y, y); // y = pt-y
if (y.Normlinf() < phys_tol) { ip = xip; return 0; }
SetIntPoint(&xip);
CalcInverse(Jacobian(), Jinv);
Jinv.Mult(y, dx);
x += dx;
prev_xip = xip;
prev_hit_bdr = hit_bdr;
xip.Set(xd, dim); // x -> xip
// If xip is ouside project it on the boundary on the line segment
// between prev_xip and xip
hit_bdr = !Geometry::ProjectPoint(geom, prev_xip, xip);
if (dx.Normlinf() < ref_tol) { ip = xip; return 0; }
if (hit_bdr)
{
xip.Get(xd, dim); // xip -> x
if (prev_hit_bdr)
{
prev_xip.Get(dxd, dim); // prev_xip -> dx
subtract(x, dx, dx); // dx = xip - prev_xip
if (dx.Normlinf() < ref_tol) { return 1; }
}
}
}
ip = xip;
return 2;
}
double IsoparametricTransformation::Weight()
{
if (FElem->GetDim() == 0)
{
return 1.0;
}
if (WeightIsEvaluated)
{
return Wght;
}
Jacobian();
WeightIsEvaluated = 1;
return (Wght = dFdx.Weight());
}
MatrixXd numericalDiff(Function& func) {
#ifndef SYMMETRIC
VectorXd y0 = func.evaluate();
#endif
// number of measurement rows
int m = func.num_measurements();
// number of variables
int n = 0;
vector<Node*>& nodes = func.nodes();
for (vector<Node*>::iterator it = nodes.begin(); it!=nodes.end(); it++) {
n += (*it)->dim();
}
// result has one column per variable
MatrixXd Jacobian(m,n);
int col = 0;
// for each node...
for (vector<Node*>::iterator it = nodes.begin(); it!=nodes.end(); it++) {
Node* node = *it;
int dim_n = node->dim();
// for each dimension of the node...
for (int j=0; j<dim_n; j++, col++) {
VectorXd delta(dim_n);
delta.setZero();
// remember original value
VectorXd original = node->vector0();
// evaluate positive delta
delta(j) = epsilon;
node->self_exmap(delta);
VectorXd y_plus = func.evaluate();
node->update0(original);
#ifdef SYMMETRIC
// evaluate negative delta
delta(j) = -epsilon;
node->self_exmap(delta);
VectorXd y_minus = func.evaluate();
node->update0(original);
// store column
VectorXd diff = (y_plus - y_minus) / (epsilon + epsilon);
#else
VectorXd diff = (y_plus - y0) / epsilon;
#endif
Jacobian.col(col) = diff;
}
}
return Jacobian;
}
//toj为目标关节的编号，一般为最末端的关节
void Kinematics::InverseKine(int toj)
{
Matrix err(6,1);
Matrix tem(6,1);
float dq[6];
const float lambda = 0.99f;
ForwardKine(0);
Matrix Jacobian(6);
for (int n=0; n<10 ; n++)
{
CalJacobian(Jacobian);
//Jacobian.PrintMatrix();
if (!Jacobian.Invert())
{
//std::cout<<"NO Jacobian Invert"<<std::endl;
}
CalErr(toj,err);
//err.PrintMatrix();
if (err.Norm()<1E-6f)
{
//TestPrint();
return;
}
tem =Jacobian * err * lambda;
//tem.PrintMatrix();
for (int n=0; n<6; n++)
dq[n] = tem.GetElement(n,0);
for (unsigned int i = 1 ; i<mJointU.size(); i++)
{
mJointU[i].q = mJointU[i].q + dq[i-1];
}
ForwardKine(0);
}
//TestPrint();
}
RVector calcGBounds( const std::vector< RVector3 > & pos, const Mesh & mesh, const RVector & model ){
/*! Ensure neighbourInfos() */
RMatrix Jacobian( pos.size(), mesh.cellCount() );
Jacobian *= 0.;
for ( uint i = 0; i < pos.size(); i ++ ){
for ( std::vector< Boundary * >::const_iterator it = mesh.boundaries().begin(); it != mesh.boundaries().end(); it ++ ){
Boundary *b = *it;
double Z = lineIntegraldGdz( b->node( 0 ).pos() - pos[ i ], b->node( 1 ).pos() - pos[ i ] );
if ( b->leftCell() ) Jacobian[ i ][ b->leftCell()->id() ] = Jacobian[ i ][ b->leftCell()->id() ] - Z;
if ( b->rightCell() ) Jacobian[ i ][ b->rightCell()->id() ] = Jacobian[ i ][ b->rightCell()->id() ] + Z;
}
}
return Jacobian * model * 2.0 * 6.67384e-11 * 1e5;
}
double CatenaryProblem::calculate_potential_energy_integrand(double x, double)
{
double potentialEnergyIntegrand = 0.0;
Vector<double> input(1);
Vector<double> output(1);
Matrix<double> Jacobian(1, 1);
input[0] = x;
output = multilayer_perceptron_pointer->calculate_output(input, *this);
double y = output[0];
Jacobian = multilayer_perceptron_pointer->calculate_Jacobian(input, *this);
double dydx = Jacobian[0][0];
potentialEnergyIntegrand = y*sqrt(1.0 + pow(dydx,2));
return(potentialEnergyIntegrand);
}
Matrix
AC3D8HexWithSensitivity::get_face_impedance(int face_num)
{
double r = 0.0;
double rw = 0.0;
double s = 0.0;
double sw = 0.0;
double weight;
double RHO, Kf, cs;
double x1,y1,z1,area;
int nodes_in_face = 8;
Matrix Cf(nodes_in_face, nodes_in_face);
Matrix Jacobian(2,3);
Matrix dh(2,nodes_in_face);
Matrix h(1,nodes_in_face);
Matrix N_coord = getFaceNodalCoords(face_num);
// Get mass density of fluid
RHO = theMaterial[0]->getRho();
if(RHO == 0.0){
opserr << "ERROR: The mass density is zero!\n";
exit(-1);
}
Kf = (theMaterial[0]->getTangent())(0,0);
cs = sqrt(Kf/RHO);
// zero matrix first
Cf.Zero();
for(short GP_c_r = 1; GP_c_r <= r_integration_order; GP_c_r++) {
r = get_Gauss_p_c(r_integration_order, GP_c_r);
rw = get_Gauss_p_w(r_integration_order, GP_c_r);
for(short GP_c_s = 1; GP_c_s <= s_integration_order; GP_c_s++) {
s = get_Gauss_p_c(s_integration_order, GP_c_s);
sw = get_Gauss_p_w(s_integration_order, GP_c_s);
dh = diff_interp_fun_face(r, s);
Jacobian = dh*N_coord;
// pointer to the fluid
x1 = Jacobian(0,1)*Jacobian(1,2) - Jacobian(0,2)*Jacobian(1,1);
y1 = Jacobian(0,2)*Jacobian(1,0) - Jacobian(0,0)*Jacobian(1,2);
z1 = Jacobian(0,0)*Jacobian(1,1) - Jacobian(0,1)*Jacobian(1,0);
// length of the bar tangent
area = sqrt(x1*x1 + y1*y1 + z1*z1);
if(area == 0.0){
opserr <<"The length of tangent should not be 0!\n";
exit(-1);
}
h = interp_fun_face(r, s);
weight = rw * sw* area / RHO /cs;
Cf.addMatrixTransposeProduct(1.0, h, h, weight);
}
}
return Cf;
}
// EulerImplicit Single step
void Integrator::EulerImplicit_Singlestep(double dt, double& t, Eigen::VectorXd& yNext) {
Eigen::MatrixXd J = Jacobian(t, yNext);
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(J.rows(),J.cols())-dt*J;
yNext+=dt*A.fullPivLu().solve(function(t,yNext));
};
void
RQMCEstimator
::initialize(MCWalkerConfiguration& W, vector<ParticleSet*>& WW,
SpaceWarp& Warp,
vector<QMCHamiltonian*>& h,
vector<TrialWaveFunction*>& psi,
RealType tau,vector<RealType>& Norm,
bool require_register) {
NumWalkers = W.getActiveWalkers();
int numPtcls(W.getTotalNum());
RatioIJ.resize(NumWalkers,NumCopies*(NumCopies-1)/2);
vector<RealType> invsumratio(NumCopies);
MCWalkerConfiguration::ParticlePos_t drift(numPtcls);
MCWalkerConfiguration::iterator it(W.begin());
MCWalkerConfiguration::iterator it_end(W.end());
vector<RealType> sumratio(NumCopies), logpsi(NumCopies);
vector<RealType> Jacobian(NumCopies);
int jindex=W.addProperty("Jacobian");
int iw(0);
int DataSetSize((*it)->DataSet.size());
while(it != it_end) {
Walker_t& thisWalker(**it);
(*it)->DataSet.rewind();
//NECESSARY SINCE THE DISTANCE TABLE OF W ARE USED TO WARP
if(require_register) {
W.registerData(thisWalker,(*it)->DataSet);
} else {
W.R = thisWalker.R;
W.update();
if(DataSetSize) W.updateBuffer((*it)->DataSet);
}
for(int ipsi=0; ipsi<NumCopies; ipsi++) Jacobian[ipsi]=1.e0;
for(int iptcl=0; iptcl< numPtcls; iptcl++){
Warp.warp_one(iptcl,0);
for(int ipsi=0; ipsi<NumCopies; ipsi++){
WW[ipsi]->R[iptcl]=W.R[iptcl]+Warp.get_displacement(iptcl,ipsi);
Jacobian[ipsi]*=Warp.get_Jacobian(iptcl,ipsi);
}
if(require_register || DataSetSize) Warp.update_one_ptcl_Jacob(iptcl);
}
for(int ipsi=0; ipsi<NumCopies; ipsi++){
thisWalker.Properties(ipsi,jindex)=Jacobian[ipsi];
}
//update distance table and bufferize it if necessary
if(require_register) {
for(int ipsi=0; ipsi<NumCopies; ipsi++){
WW[ipsi]->registerData((*it)->DataSet);
}
Warp.registerData(WW,(*it)->DataSet);
} else {
for(int ipsi=0; ipsi<NumCopies; ipsi++){
WW[ipsi]->update();
if(DataSetSize) WW[ipsi]->updateBuffer((*it)->DataSet);
}
if(DataSetSize) Warp.updateBuffer((*it)->DataSet);
}
//evalaute the wavefunction and hamiltonian
for(int ipsi=0; ipsi< NumCopies;ipsi++) {
psi[ipsi]->G.resize(numPtcls);
psi[ipsi]->L.resize(numPtcls);
//Need to modify the return value of OrbitalBase::registerData
if(require_register) {
logpsi[ipsi]=psi[ipsi]->registerData(*WW[ipsi],(*it)->DataSet);
}else{
if(DataSetSize)logpsi[ipsi]=psi[ipsi]->updateBuffer(*WW[ipsi],(*it)->DataSet);
else logpsi[ipsi]=psi[ipsi]->evaluateLog(*WW[ipsi]);
}
psi[ipsi]->G=WW[ipsi]->G;
thisWalker.Properties(ipsi,LOGPSI)=logpsi[ipsi];
thisWalker.Properties(ipsi,LOCALENERGY)=h[ipsi]->evaluate(*WW[ipsi]);
h[ipsi]->saveProperty(thisWalker.getPropertyBase(ipsi));
sumratio[ipsi]=1.0;
}
//Check SIMONE's note
//Compute the sum over j of Psi^2[j]/Psi^2[i] for each i
int indexij(0);
RealType *rPtr=RatioIJ[iw];
for(int ipsi=0; ipsi< NumCopies-1; ipsi++) {
for(int jpsi=ipsi+1; jpsi< NumCopies; jpsi++){
RealType r= std::exp(2.0*(logpsi[jpsi]-logpsi[ipsi]))*Norm[ipsi]/Norm[jpsi];
//BEWARE: RatioIJ DOES NOT INCLUDE THE JACOBIANS!
rPtr[indexij++]=r;
r*=(Jacobian[jpsi]/Jacobian[ipsi]);
sumratio[ipsi] += r;
sumratio[jpsi] += 1.0/r;
}
}
//Re-use Multiplicity as the sumratio
thisWalker.Multiplicity=sumratio[0];
/*START COMMENT
QMCTraits::PosType WarpDrift;
RealType denom(0.e0),wgtpsi;
thisWalker.Drift=0.e0;
for(int ipsi=0; ipsi< NumCopies; ipsi++) {
wgtpsi=1.e0/sumratio[ipsi];
thisWalker.Properties(ipsi,UMBRELLAWEIGHT)=wgtpsi;
denom += wgtpsi;
for(int iptcl=0; iptcl< numPtcls; iptcl++){
WarpDrift=dot( psi[ipsi]->G[iptcl], Warp.get_Jacob_matrix(iptcl,ipsi) )
+5.0e-1*Warp.get_grad_ln_Jacob(iptcl,ipsi) ;
thisWalker.Drift[iptcl] += (wgtpsi*WarpDrift);
}
}
//Drift = denom*Drift;
thisWalker.Drift *= (tau/denom);
END COMMENT*/
for(int ipsi=0; ipsi< NumCopies ;ipsi++){
invsumratio[ipsi]=1.0/sumratio[ipsi];
thisWalker.Properties(ipsi,UMBRELLAWEIGHT)=invsumratio[ipsi];
}
setScaledDrift(tau,psi[0]->G,drift);
thisWalker.Drift=invsumratio[0]*drift;
for(int ipsi=1; ipsi< NumCopies ;ipsi++) {
setScaledDrift(tau,psi[ipsi]->G,drift);
thisWalker.Drift += (invsumratio[ipsi]*drift);
}
++it;++iw;
}
}
void GslInternal::init(){
// Init ODE rhs function and quadrature functions
f_.init();
casadi_assert(f_.getNumInputs()==DAE_NUM_IN);
casadi_assert(f_.getNumOutputs()==DAE_NUM_OUT);
if(!q_.isNull()){
q_.init();
casadi_assert(q_.getNumInputs()==DAE_NUM_IN);
casadi_assert(q_.getNumOutputs()==DAE_NUM_OUT);
}
// Number of states
int nx = f_.output(INTEGRATOR_XF).numel();
// Add quadratures, if any
if(!q_.isNull()) nx += q_.output().numel();
// Number of parameters
int np = f_.input(DAE_P).numel();
setDimensions(nx,np);
// If time was not specified, initialise it.
if (f_.input(DAE_T).numel()==0) {
std::vector<MX> in1(DAE_NUM_IN);
in1[DAE_T] = MX("T");
in1[DAE_Y] = MX("Y",f_.input(DAE_Y).size1(),f_.input(DAE_Y).size2());
in1[DAE_YDOT] = MX("YDOT",f_.input(DAE_YDOT).size1(),f_.input(DAE_YDOT).size2());
in1[DAE_P] = MX("P",f_.input(DAE_P).size1(),f_.input(DAE_P).size2());
std::vector<MX> in2(in1);
in2[DAE_T] = MX();
f_ = MXFunction(in1,f_.call(in2));
f_.init();
}
// We only allow for 0-D time
casadi_assert_message(f_.input(DAE_T).numel()==1, "IntegratorInternal: time must be zero-dimensional, not (" << f_.input(DAE_T).size1() << 'x' << f_.input(DAE_T).size2() << ")");
// ODE right hand side must be a dense matrix
casadi_assert_message(f_.output(DAE_RES).dense(),"ODE right hand side must be dense: reformulate the problem");
// States and RHS should match
casadi_assert_message(f_.output(DAE_RES).size()==f_.input(DAE_Y).size(),
"IntegratorInternal: rhs of ODE is (" << f_.output(DAE_RES).size1() << 'x' << f_.output(DAE_RES).size2() << ") - " << f_.output(DAE_RES).size() << " non-zeros" << std::endl <<
" ODE state matrix is (" << f_.input(DAE_Y).size1() << 'x' << f_.input(DAE_Y).size2() << ") - " << f_.input(DAE_Y).size() << " non-zeros" << std::endl <<
"Mismatch between number of non-zeros"
);
IntegratorInternal::init();
jac_f_ = Jacobian(f_,DAE_Y,DAE_RES);
dt_f_ = Jacobian(f_,DAE_T,DAE_RES);
jac_f_.init();
dt_f_.init();
// define the type of routine for making steps:
type_ptr = gsl_odeiv_step_rkf45;
// some other possibilities (see GSL manual):
// = gsl_odeiv_step_rk4;
// = gsl_odeiv_step_rkck;
// = gsl_odeiv_step_rk8pd;
// = gsl_odeiv_step_rk4imp;
// = gsl_odeiv_step_bsimp;
// = gsl_odeiv_step_gear1;
// = gsl_odeiv_step_gear2;
step_ptr = gsl_odeiv_step_alloc (type_ptr, nx);
control_ptr = gsl_odeiv_control_y_new (abstol_, reltol_);
evolve_ptr = gsl_odeiv_evolve_alloc (nx);
my_system.function = rhs_wrapper; // the right-hand-side functions dy[i]/dt
my_system.jacobian = jac_wrapper; // the Jacobian df[i]/dy[j]
my_system.dimension = nx; // number of diffeq's
my_system.params = this; // parameters to pass to rhs and jacobian
is_init = true;
}
void Rsc::CalcLambda(const itpp::cvec &received, const itpp::vec &logLikelihood_in, double n0,
bool knowLastState) const
{
itpp::BPSK_c bpsk;
const int branchNum = received.size() * codeRate_.numerator() / codeRate_.denominator(); // レートは1/2固定
const int nodeNum = branchNum + 1;
itpp::mat alpha(nodeNum, stateNum_), beta(nodeNum, stateNum_);
std::vector< itpp::mat > gamma(branchNum, itpp::mat(stateNum_, 2)); // [a](b,c)で時刻aの状態bに入力c
lambda_.set_size(branchNum); // デコードし終わったときのLambda
itpp::mat logPrioriProb(branchNum, 2);
// p.162の下部
for (int i = 0; i < branchNum; ++i){
// double t_exp = std::exp(logLikelihood_in[i]);
logPrioriProb(i, 0) = -Jacobian(0, logLikelihood_in[i]);
// std::log(1.0 + t_exp);
logPrioriProb(i, 1) = logLikelihood_in[i] + logPrioriProb(i, 0);
} // for i
alpha.zeros();
beta.zeros();
// for (int i = 0; i < nodeNum; ++i){
for (int state = 0; state < stateNum_; ++state){
alpha(0, state) = -mylib::INFTY;
beta(nodeNum - 1, state) = -mylib::INFTY;
} // for state
// } // for i
alpha(0,0) = 0;
if (knowLastState){
beta(nodeNum - 1, lastState_) = 0;
} // if
else{
for (int state = 0; state < stateNum_; ++state){
beta(nodeNum - 1, state) = 0;
} // for state
} // else
// gamma
{
itpp::bvec originalCode(codeRate_.denominator()); // 元の送信信号
for (int i = 0; i < branchNum; ++i){
for (int state = 0; state < stateNum_; ++state){
for (int bit = 0; bit < 2; ++bit){
originalCode[0] = bit;
originalCode[1] = encodeTable_[state][bit].output_;
itpp::cvec originalTransSymbol = bpsk.modulate_bits(originalCode);
double distance = itpp::sqr(std::real(originalTransSymbol[0] - received[2*i])) +
itpp::sqr(std::real(originalTransSymbol[1] - received[2*i + 1]));
gamma[i](state, bit) = logPrioriProb(i, bit) - distance / n0;
} // for bit
} // for state
} // for i
} // gamma
// boost::thread alphaThread = boost::thread(boost::bind(&Rsc::CalcAlpha, this, &alpha, gamma, nodeNum));
// boost::thread betaThread = boost::thread(boost::bind(&Rsc::CalcBeta, this, &beta, gamma, nodeNum));
// alphaThread.join();
// betaThread.join();
// alpha
for (int i = 1; i < nodeNum; ++i){
for (int state = 0; state < stateNum_; ++state){
// for (int bit = 0; bit < 2; ++bit){
int primState0 = revEncodeTable_[state][0];
int primState1 = revEncodeTable_[state][1];
alpha(i, state) = Jacobian(alpha(i-1, primState0) + gamma[i-1](primState0, 0),
alpha(i-1, primState1) + gamma[i-1](primState1, 1));
// } // for bit
} // for state
} // for i
// beta
for (int i = nodeNum - 2; i >= 0; --i){
for (int state = 0; state < stateNum_; ++state){
// for (int bit = 0; bit < 2; ++bit){
int nextState0 = encodeTable_[state][0].nextState_;
int nextState1 = encodeTable_[state][1].nextState_;
beta(i, state) = Jacobian(beta(i+1, nextState0) + gamma[i](state, 0),
beta(i+1, nextState1) + gamma[i](state, 1));
// } // for bit
} // for state
} // for i
// lambda_
for (int i = 1; i < nodeNum; ++i){
itpp::vec likelihood(2);
likelihood[0] = likelihood[1] = -mylib::INFTY;
for (int state = 0; state < stateNum_; ++state){
for (int bit = 0; bit < 2; ++bit){
int nextState = encodeTable_[state][bit].nextState_;
double t_delta = alpha(i-1, state) + gamma[i-1](state, bit)
+ beta(i, nextState);
likelihood[bit] = Jacobian(likelihood[bit], t_delta);
} // for bit
} // for state
lambda_[i - 1] = likelihood[1] - likelihood[0];
} // for i
}
