Matrix Matrix::estimate_quaternion(Matrix& A, Matrix& B, Matrix& A2, Matrix& B2) {
Matrix N1 = A.cross(B);
N1.normalize();
Matrix N2 = A2.cross(B2);
N2.normalize();
double cosa = N1.dot(N2.transposed()).get(0,0);
double sina = sqrt(0.5 - 0.5*cosa);
cosa = sqrt(0.5 + 0.5*cosa);
Matrix NN = N1.cross(N2);
NN.normalize();
NN *= sina;
double Q1_[] = {cosa, NN(0,0), NN(0,1), NN(0,2)};
Matrix Q1(1,4,Q1_);
A = A.quaternion_rotate(Q1);
B = B.quaternion_rotate(Q1);
cosa = A.dot(A2.transposed()).get(0,0);
sina = sqrt(0.5 - 0.5*cosa);
cosa = sqrt(0.5 + 0.5*cosa);
Matrix ax2 = A.cross(A2);
ax2.normalize();
ax2 *= sina;
double Q2_[] = {cosa, ax2(0,0), ax2(0,1), ax2(0,2)};
Matrix Q2(1, 4, Q2_);
Matrix Q = Q2.quaternion_multiply(Q1);
return Q;
}
const Vector&
PFEMElement3D::getResistingForceIncInertia()
{
// resize P
int ndf = this->getNumDOF();
P.resize(ndf);
P.Zero();
// get velocity, accleration
Vector v(ndf), vdot(ndf);
for(int i=0; i<4; i++) {
const Vector& accel = nodes[2*i]->getTrialAccel();
vdot(numDOFs(2*i)) = accel(0);
vdot(numDOFs(2*i)+1) = accel(1);
vdot(numDOFs(2*i)+2) = accel(2);
const Vector& vel = nodes[2*i]->getTrialVel();
v(numDOFs(2*i)) = vel(0);
v(numDOFs(2*i)+1) = vel(1);
v(numDOFs(2*i)+2) = vel(2);
const Vector& vel2 = nodes[2*i+1]->getTrialVel();
v(numDOFs(2*i+1)) = vel2(0);
v(numDOFs(2*i+1)+1) = vel2(1);
v(numDOFs(2*i+1)+2) = vel2(2);
v(numDOFs(2*i+1)+3) = vel2(3);
}
double d = v.Norm();
if(d!=d) opserr<<"v "<<this->getTag()<<"\n";
d = vdot.Norm();
if(d!=d) opserr<<"vdot "<<this->getTag()<<"\n";
P.addMatrixVector(1.0, getMass(), vdot, 1.0);
P.addMatrixVector(1.0, getDampWithK(), v, 1.0);
Vector ones(ndf);
for(int i=0;i<ndf;i++) ones(i) = 1.0;
Vector Q1(ndf), Q2(ndf);
Q1.addMatrixVector(1.0, getMass(), ones, 1.0);
Q2.addMatrixVector(1.0, getDampWithK(), ones, 1.0);
d = Q1.Norm();
if(d!=d) opserr<<"mass "<<this->getTag()<<"\n";
d = Q2.Norm();
if(d!=d) {
opserr<<"damp "<<this->getTag()<<"\n";
exit(1);
}
// get Jacobi
for(int i=0; i<4; i++) {
P(numDOFs(2*i)) -= rho*bx/24.*J;
P(numDOFs(2*i)+1) -= rho*by/24.*J;
P(numDOFs(2*i)+2) -= rho*bz/24.*J;
}
return P;
}
/*! main */
int main(int argc, char** argv)
{
srand(time(NULL));
int M(4), N(4), KL(2), KU(1);
//int M(2), N(2), KL(0), KU(0);
CPPL::zgbmatrix A(M,N,KL,KU);
for(long i=0; i<A.m; i++){
for(long j=std::max(long(0),i-A.kl); j<std::min(A.n,i+A.ku+1); j++){
A(i,j) =std::complex<double>(rand()/(RAND_MAX/10), rand()/(RAND_MAX/10));
}
}
std::cout << "A =\n" << A << std::endl;
CPPL::zgbmatrix B;
std::cout << "#### B=A; ####" << std::endl;
B=A;
std::cout << "B =\n" << B << std::endl;
std::cout << "#### B+=A; ####" << std::endl;
B+=A;
std::cout << "B =\n" << B << std::endl;
std::cout << "#### B-=A; ####" << std::endl;
B-=A;
std::cout << "B =\n" << B << std::endl;
std::cout << "A+B =\n" << A+B << std::endl;
std::cout << "A-B =\n" << A-B << std::endl;
std::cout << "A*B =\n" << A*B << std::endl;
CPPL::zgbmatrix P(8,10,2,3), Q(10,9,1,3), R;
for(long i=0; i<P.m; i++){
for(long j=std::max(long(0),i-P.kl); j<std::min(P.n,i+P.ku+1); j++){
P(i,j) =std::complex<double>(rand()/(RAND_MAX/10), rand()/(RAND_MAX/10));
}
}
for(long i=0; i<Q.m; i++){
for(long j=std::max(long(0),i-Q.kl); j<std::min(Q.n,i+Q.ku+1); j++){
Q(i,j) =std::complex<double>(rand()/(RAND_MAX/10), rand()/(RAND_MAX/10));
}
}
CPPL::zgematrix P2( P.to_zgematrix() ), Q2( Q.to_zgematrix() );
std::cout << "P =\n" << P << std::endl;
std::cout << "P2 =\n" << P2 << std::endl;
std::cout << "Q =\n" << Q << std::endl;
std::cout << "Q2 =\n" << Q2 << std::endl;
std::cout << "P*Q =\n" << P*Q << std::endl;
std::cout << "P2*Q2 =\n" << P2*Q2 << std::endl;
std::cout << "P*Q - P2*Q2 =\n" << P*Q-P2*Q2 << std::endl;
std::cout << "#### P*=Q; ####" << std::endl;
P*=Q;
std::cout << "P =\n" << P << std::endl;
return 0;
}
/** Performs the Kratky transformation: IQ^2 v Q
* @param ws The workspace to be transformed
*/
void IQTransform::kratky(API::MatrixWorkspace_sptr ws)
{
MantidVec& X = ws->dataX(0);
MantidVec& Y = ws->dataY(0);
MantidVec& E = ws->dataE(0);
MantidVec Q2(X.size());
std::transform(X.begin(),X.end(),Q2.begin(),VectorHelper::Squares<double>());
std::transform(Y.begin(),Y.end(),Q2.begin(),Y.begin(),std::multiplies<double>());
std::transform(E.begin(),E.end(),Q2.begin(),E.begin(),std::multiplies<double>());
ws->setYUnitLabel("I x Q^2");
}
// -----------------------------------------
// tests
// -----------------------------------------
TEST(ExtendedKalmanFilterTest, initialization)
{
ExtendedKalmanFilter ekf;
// check setting required params
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "STM missing"
ekf.setStateTransitionModel(f);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "Jacobian of STM missing"
ekf.setJacobianOfStateTransitionModel(df);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "OM missing"
ekf.setObservationModel(h);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "Jacobian of OM missing"
ekf.setJacobianOfObservationModel(dh);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "PNC missing"
MatrixXd Q(1,1); Q << 0.1;
ekf.setProcessNoiseCovariance(Q);
EXPECT_THROW(ekf.validate(), IEstimator::estimator_error); // "MNC missing"
MatrixXd R(1,1); R << 10;
ekf.setMeasurementNoiseCovariance(R);
EXPECT_NO_THROW(ekf.validate()); // all required params given
// check setting optional params
// optional params doesn't set new sizes
MatrixXd P0_f(2,2); P0_f << 1, 0, 0, 1;
ekf.setInitialErrorCovariance(P0_f);
EXPECT_NO_THROW(ekf.validate());
EXPECT_EQ(ekf.getState().size(), 1);
VectorXd x_f(2); x_f << 0,1;
ekf.setInitialState(x_f);
EXPECT_NO_THROW(ekf.validate());
EXPECT_NE(ekf.getState().size(), 2);
// required param Q sets new size of x and out
MatrixXd Q2(2,2); Q2 << 0.1, 0, 0, 0.1;
ekf.setProcessNoiseCovariance(Q2);
EXPECT_NO_THROW(ekf.validate());
EXPECT_EQ(ekf.getState().size(), 2);
EXPECT_EQ(ekf.getLastEstimate().size(), 2);
// check initialization of output
Output out = ekf.getLastEstimate();
OutputValue defaultOutVal;
EXPECT_GT(out.size(), 0);
EXPECT_DOUBLE_EQ(out.getValue(), defaultOutVal.getValue());
// check setting the control input
InputValue ctrl(0);
Input in_ctrl(ctrl);
EXPECT_THROW(ekf.setControlInput(in_ctrl), length_error);
ekf.setControlInputSize(1);
EXPECT_NO_THROW(ekf.setControlInput(in_ctrl));
}
void FORTE_FB_HYST_3::alg_REQ(void){
CIEC_REAL X;
X = HYS()*0.5f;
if((IN() < VAL1()-X)){
Q1() = true;
}
else
if((IN() > VAL1() + X)){
Q1() = false;
};
if((IN() < VAL2()-X)){
Q2() = false;
}
else
if((IN() > VAL2()+X)){
Q2() = true;
};
}
//---------------------------------------------------------
void MaxwellCurved2D::InitRun()
//---------------------------------------------------------
{
StartUp2D(); // construct grid and metric
#if (0)
umLOG(1, "before refine : K = %5d\n", K);
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
DMat Q2(Np*K, 1); IMat refineflag;
refineflag = Ones(K,Nfaces); Q2 = ConformingHrefine2D(refineflag, Q2);
umLOG(1, "after refine 1: K = %5d\n", K);
//refineflag = Ones(K,Nfaces); Q2 = ConformingHrefine2D(refineflag, Q2);
//umLOG(1, "after refine 1: K = %5d\n", K);
//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
#endif
BuildBCMaps2D(); // build boundary condition maps
//OutputNodes(true); // face nodes
AdjustCylBC(1., 0.,0., BC_All); // Push all boundary faces to unit cylinder
//OutputNodes(true); // face nodes
Resize(); // allocate work arrays
SetIC(); // set initial conditions
SetStepSize(); // calculate step size (dt)
BuildCurvedOPS2D(3*N);
//---------------------------------------------
// base class version sets counters and flags
//---------------------------------------------
NDG2D::InitRun();
//---------------------------------------------
// Adjust reporting and render frequencies
//---------------------------------------------
Nreport = Nsteps/20;
//Nreport = 2; // set frequency of reporting (param)
//Nreport = 10; // set frequency of reporting (param)
//Nreport = 50; // set frequency of reporting (param)
Nrender = Nreport; // output frequency (param)
//Nrender = 10; // output frequency (param)
//Nrender = 100000; // output frequency (param)
NvtkInterp = 12; // set output resolution
//NvtkInterp = 6; // set output resolution
Summary(); // Show simulation details
}
int main ()
{
struct node *Q1head;
printf("Q1 OUTPUT:\n\n");
Q1head=Q1();
printf("Q2 OUTPUT:\n\n");
Q2(Q1head);
printf("Q3 OUTPUT:\n\n");
Q3();
system("pause>nul");
}
void extr(jvec &ext_EP,jvec &ext_ED,jvec &ext_Q2,jvec &ext_fP,jvec &ext_fM,jvec &ext_f0,jvec &ext_fT,int il_sea,int il,int ic)
{
////////////////////////////////////////// R0 //////////////////////////////////////
jvec R0_corr;
jack R0(njack);
//load standing
jvec ll0_st=load_3pts("V0",il,il,0,RE,ODD,1);
jvec lc0_st=load_3pts("V0",ic,il,0,RE,ODD,1);
jvec cc0_st=load_3pts("V0",ic,ic,0,RE,ODD,1);
//build R0
R0_corr=lc0_st*lc0_st.simmetric()/(cc0_st*ll0_st);
//fit and plot
R0=constant_fit(R0_corr,TH-tmax,tmax,combine("plots/R0_il_%d_ic_%d.xmg",il,ic).c_str());
//////////////////////////////////////////// R2 ////////////////////////////////////
jvec R2_corr[nth];
jvec RT_corr[nth];
jvec R2(nth,njack);
jvec RT(nth,njack);
ofstream out_R2(combine("plots/R2_il_%d_ic_%d.xmg",il,ic).c_str());
ofstream out_RT(combine("plots/RT_il_%d_ic_%d.xmg",il,ic).c_str());
jvec lcK_th[nth],lc0_th[nth],lcT_th[nth];
for(int ith=0;ith<nth;ith++)
{
//load corrs
lcK_th[ith]=load_3pts("VK",ic,il,ith,IM,EVN,-1)/(6*th_P[ith]);
lc0_th[ith]=load_3pts("V0",ic,il,ith,RE,ODD,1);
lcT_th[ith]=load_3pts("VTK",ic,il,ith,IM,ODD,1)/(6*th_P[ith]);
//build ratios
R2_corr[ith]=lcK_th[ith]/lc0_th[ith];
RT_corr[ith]=lcT_th[ith]/lcK_th[ith];
//fit
R2[ith]=constant_fit(R2_corr[ith],tmin,tmax);
RT[ith]=constant_fit(RT_corr[ith],tmin,tmax);
//plot
out_R2<<write_constant_fit_plot(R2_corr[ith],R2[ith],tmin,tmax);
out_RT<<write_constant_fit_plot(RT_corr[ith],RT[ith],tmin,tmax);
}
////////////////////////////////////////// R1 //////////////////////////////////////
jvec R1_corr[nth];
jvec R1(nth,njack);
ofstream out_P(combine("plots/out_P_il_%d_ic_%d.xmg",il,ic).c_str());
out_P<<"@type xydy"<<endl;
ofstream out_D(combine("plots/out_D_il_%d_ic_%d.xmg",il,ic).c_str());
out_D<<"@type xydy"<<endl;
ofstream out_R1(combine("plots/out_R1_il_%d_ic_%d.xmg",il,ic).c_str());
out_R1<<"@type xydy"<<endl;
//load Pi and D
jvec P_corr[nth],D_corr[nth];
jvec ED(nth,njack),EP(nth,njack);
for(int ith=0;ith<nth;ith++)
{
//load moving pion
P_corr[ith]=load_2pts("2pts_P5P5.dat",il_sea,il,ith);
out_P<<"@type xydy"<<endl;
EP[ith]=constant_fit(effective_mass(P_corr[ith]),tmin_P,TH,combine("plots/P_eff_mass_il_%d_ic_%d_ith_%d.xmg",
il,ic,ith).c_str());
out_P<<write_constant_fit_plot(effective_mass(P_corr[ith]),EP[ith],tmin_P,TH);
out_P<<"&"<<endl;
//recompute EP and ED from standing one
if(ith)
{
ED[ith]=latt_en(ED[0],th_P[ith]);
EP[ith]=latt_en(EP[0],th_P[ith]);
}
//load moving D
D_corr[ith]=load_2pts("2pts_P5P5.dat",il,ic,ith);
out_D<<"@type xydy"<<endl;
ED[ith]=constant_fit(effective_mass(D_corr[ith]),tmin_D,TH,combine("plots/D_eff_mass_il_%d_ic_%d_ith_%d.xmg",
il,ic,ith).c_str());
out_D<<write_constant_fit_plot(effective_mass(D_corr[ith]),ED[ith],tmin_D,TH);
out_D<<"&"<<endl;
//build the ratio
R1_corr[ith]=lc0_th[ith]/lc0_th[0];
for(int t=0;t<TH;t++)
{
int E_fit_reco_flag=1;
jack Dt(njack),Pt(njack);
if(E_fit_reco_flag==0)
{
Dt=D_corr[0][t]/D_corr[ith][t];
Pt=P_corr[0][TH-t]/P_corr[ith][TH-t];
}
else
{
jack ED_th=latt_en(ED[0],th_P[ith]),EP_th=latt_en(EP[0],th_P[ith]);
Dt=exp(-(ED[0]-ED_th)*t)*ED_th/ED[0];
Pt=exp(-(EP[0]-EP_th)*(TH-t))*EP_th/EP[0];
}
R1_corr[ith][t]*=Dt*Pt;
}
//fit
R1[ith]=constant_fit(R1_corr[ith],tmin,tmax);
//plot
out_R1<<write_constant_fit_plot(R1_corr[ith],R1[ith],tmin,tmax);
}
//////////////////////////////////////// solve the ratios //////////////////////////////
//compute f0[q2max]
jvec f0_r(nth,njack),fP_r(nth,njack),fT_r(nth,njack);
f0_r[0]=sqrt(R0*4*ED[0]*EP[0])/(ED[0]+EP[0]);
cout<<"f0_r[q2max]: "<<f0_r[0]<<endl;
//compute QK and Q2
double mom[nth];
jvec PK(nth,njack),QK(nth,njack);
jvec P0(nth,njack),Q0(nth,njack),Q2(nth,njack),P2(nth,njack);
jvec P0_r(nth,njack),Q0_r(nth,njack),Q2_r(nth,njack),P2_r(nth,njack);
for(int ith=0;ith<nth;ith++)
{
P0[ith]=ED[ith]+EP[ith]; //P=initial+final
Q0[ith]=ED[ith]-EP[ith]; //Q=initial-final
P0_r[ith]=latt_en(ED[0],th_P[ith])+latt_en(EP[0],th_P[ith]);
Q0_r[ith]=latt_en(ED[0],th_P[ith])-latt_en(EP[0],th_P[ith]);
//we are describing the process D->Pi
mom[ith]=momentum(th_P[ith]);
double P_D=-mom[ith];
double P_Pi=mom[ith];
PK[ith]=P_D+P_Pi;
QK[ith]=P_D-P_Pi;
P2[ith]=sqr(P0[ith])-3*sqr(PK[ith]);
Q2[ith]=sqr(Q0[ith])-3*sqr(QK[ith]);
//reconstruct Q2
P2_r[ith]=sqr(P0_r[ith])-3*sqr(PK[ith]);
Q2_r[ith]=sqr(Q0_r[ith])-3*sqr(QK[ith]);
}
//checking Pion dispertion relation
ofstream out_disp_P(combine("plots/Pion_disp_rel_il_%d_ic_%d.xmg",il,ic).c_str());
out_disp_P<<"@type xydy"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(EP[ith])<<endl;
out_disp_P<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(cont_en(EP[0],th_P[ith]))<<endl;
out_disp_P<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_P<<3*sqr(mom[ith])<<" "<<sqr(latt_en(EP[0],th_P[ith]))<<endl;
out_disp_P<<"&"<<endl;
//checking D dispertion relation
ofstream out_disp_D(combine("plots/D_disp_rel_il_%d_ic_%d.xmg",il,ic).c_str());
out_disp_D<<"@type xydy"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(ED[ith])<<endl;
out_disp_D<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(cont_en(ED[0],th_P[ith]))<<endl;
out_disp_D<<"&"<<endl;
for(int ith=0;ith<nth;ith++) out_disp_D<<3*sqr(mom[ith])<<" "<<sqr(latt_en(ED[0],th_P[ith]))<<endl;
out_disp_D<<"&"<<endl;
//compute xi
jvec xi(nth,njack);
for(int ith=1;ith<nth;ith++)
{
int E_fit_reco_flag=0; //it makes no diff
jack P0_th=E_fit_reco_flag?P0_r[ith]:P0[ith];
jack Q0_th=E_fit_reco_flag?Q0_r[ith]:Q0[ith];
xi[ith]=R2[ith]*P0_th;
xi[ith]/=QK[ith]-R2[ith]*Q0_th;
}
//compute fP
ofstream out_fP_r(combine("plots/fP_r_il_%d_ic_%d.xmg",il,ic).c_str());
out_fP_r<<"@type xydy"<<endl;
for(int ith=1;ith<nth;ith++)
{
int E_fit_reco_flag=1; //it makes no diff
jack P0_th=E_fit_reco_flag?P0_r[ith]:P0[ith];
jack Q0_th=E_fit_reco_flag?Q0_r[ith]:Q0[ith];
jack c=P0_th/(ED[0]+EP[0])*(1+xi[ith]*Q0_th/P0_th);
fP_r[ith]=R1[ith]/c*f0_r[0];
out_fP_r<<Q2[ith].med()<<" "<<fP_r[ith]<<endl;
}
//compute f0 and fT
ofstream out_f0_r(combine("plots/f0_r_il_%d_ic_%d.xmg",il,ic).c_str());
ofstream out_fT_r(combine("plots/fT_r_il_%d_ic_%d.xmg",il,ic).c_str());;
out_f0_r<<"@type xydy"<<endl;
out_f0_r<<Q2[0].med()<<" "<<f0_r[0]<<endl;
out_fT_r<<"@type xydy"<<endl;
for(int ith=1;ith<nth;ith++)
{
//it seems better here to solve using reconstructed energies
int E_fit_reco_flag=0;
jack EP_th=E_fit_reco_flag?latt_en(EP[0],th_P[ith]):EP[ith];
jack ED_th=E_fit_reco_flag?latt_en(ED[0],th_P[ith]):ED[ith];
jack Q2_th=E_fit_reco_flag?Q2_r[ith]:Q2[ith];
jack fM_r=xi[ith]*fP_r[ith]; //checked
f0_r[ith]=fP_r[ith]+fM_r[ith]*Q2_th/(sqr(ED_th)-sqr(EP_th));
out_f0_r<<Q2[ith].med()<<" "<<f0_r[ith]<<endl;
fT_r[ith]=fM_r[ith]*RT[ith]*Zt_med[ibeta]/Zv_med[ibeta]*(EP[0]+ED[0])/(ED[ith]+EP[ith]); //ADD
out_fT_r<<Q2[ith].med()<<" "<<fT_r[ith]<<endl;
}
//////////////////////////////////////// analytic method /////////////////////////////
jvec fP_a(nth,njack),fM_a(nth,njack),f0_a(nth,njack),fT_a(nth,njack);
jvec fP_n(nth,njack),fM_n(nth,njack),f0_n(nth,njack),fT_n(nth,njack);
//determine M and Z for pion and D
jvec ZP(nth,njack),ZD(nth,njack);
for(int ith=0;ith<nth;ith++)
{
jack E,Z2;
two_pts_fit(E,Z2,P_corr[ith],tmin_P,TH);
ZP[ith]=sqrt(Z2);
two_pts_fit(E,Z2,D_corr[ith],tmin_D,TH);
ZD[ith]=sqrt(Z2);
}
//compute V
jvec VK_a(nth,njack),V0_a(nth,njack),TK_a(nth,njack);
jvec VK_n(nth,njack),V0_n(nth,njack),TK_n(nth,njack);
for(int ith=0;ith<nth;ith++)
{
ofstream out_V0(combine("plots/V0_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_V0<<"@type xydy"<<endl;
ofstream out_VK(combine("plots/VK_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_VK<<"@type xydy"<<endl;
ofstream out_TK(combine("plots/TK_il_%d_ic_%d_ith_%d_analytic_numeric.xmg",il,ic,ith).c_str());
out_TK<<"@type xydy"<<endl;
ofstream out_dt(combine("plots/dt_il_%d_ic_%d_ith_%d.xmg",il,ic,ith).c_str());
out_dt<<"@type xydy"<<endl;
//computing time dependance
jvec dt_a(TH+1,njack),dt_n(TH+1,njack);
{
//it seems better here to use fitted energies
int E_fit_reco_flag=1;
jack EP_th=E_fit_reco_flag?latt_en(EP[0],th_P[ith]):EP[ith];
jack ED_th=E_fit_reco_flag?latt_en(ED[0],th_P[ith]):ED[ith];
for(int t=0;t<=TH;t++)
{
dt_a[t]=exp(-(ED_th*t+EP_th*(TH-t)))*ZP[0]*ZD[0]/(4*EP_th*ED_th);
dt_n[t]=D_corr[ith][t]*P_corr[ith][TH-t]/(ZD[0]*ZP[0]);
}
}
//remove time dependance using analytic or numeric expression
jvec VK_corr_a=Zv_med[ibeta]*lcK_th[ith]/dt_a,V0_corr_a=Zv_med[ibeta]*lc0_th[ith]/dt_a;
jvec VK_corr_n=Zv_med[ibeta]*lcK_th[ith]/dt_n,V0_corr_n=Zv_med[ibeta]*lc0_th[ith]/dt_n;
jvec TK_corr_n=Zt_med[ibeta]*lcT_th[ith]/dt_n,TK_corr_a=Zt_med[ibeta]*lcT_th[ith]/dt_a;
//fit V0
V0_a[ith]=constant_fit(V0_corr_a,tmin,tmax);
V0_n[ith]=constant_fit(V0_corr_n,tmin,tmax);
out_V0<<write_constant_fit_plot(V0_corr_a,V0_a[ith],tmin,tmax)<<"&"<<endl;
out_V0<<write_constant_fit_plot(V0_corr_n,V0_n[ith],tmin,tmax)<<"&"<<endl;
//fit VK
VK_a[ith]=constant_fit(VK_corr_a,tmin,tmax);
VK_n[ith]=constant_fit(VK_corr_n,tmin,tmax);
out_VK<<write_constant_fit_plot(VK_corr_a,VK_a[ith],tmin,tmax)<<"&"<<endl;
out_VK<<write_constant_fit_plot(VK_corr_n,VK_n[ith],tmin,tmax)<<"&"<<endl;
//fit TK
TK_a[ith]=constant_fit(TK_corr_a,tmin,tmax);
TK_n[ith]=constant_fit(TK_corr_n,tmin,tmax);
out_TK<<write_constant_fit_plot(TK_corr_a,TK_a[ith],tmin,tmax)<<"&"<<endl;
out_TK<<write_constant_fit_plot(TK_corr_n,TK_n[ith],tmin,tmax)<<"&"<<endl;
}
//compute f0(q2max)
f0_a[0]=V0_a[0]/(ED[0]+EP[0]);
f0_n[0]=V0_n[0]/(ED[0]+EP[0]);
cout<<"f0_a["<<Q2[0].med()<<"]: "<<f0_a[0]<<endl;
cout<<"f0_n["<<Q2[0].med()<<"]: "<<f0_n[0]<<endl;
//solve for fP and f0
for(int ith=1;ith<nth;ith++)
{
jack delta=P0[ith]*QK[ith]-Q0[ith]*PK[ith];
//solve using analytic fit
jack deltaP_a=V0_a[ith]*QK[ith]-Q0[ith]*VK_a[ith];
jack deltaM_a=P0[ith]*VK_a[ith]-V0_a[ith]*PK[ith];
fP_a[ith]=deltaP_a/delta;
fM_a[ith]=deltaM_a/delta;
//solve using numeric fit
jack deltaP_n=V0_n[ith]*QK[ith]-Q0[ith]*VK_n[ith];
jack deltaM_n=P0[ith]*VK_n[ith]-V0_n[ith]*PK[ith];
fP_n[ith]=deltaP_n/delta;
fM_n[ith]=deltaM_n/delta;
//compute f0
f0_a[ith]=fP_a[ith]+fM_a[ith]*Q2[ith]/(ED[0]*ED[0]-EP[0]*EP[0]);
f0_n[ith]=fP_n[ith]+fM_n[ith]*Q2[ith]/(ED[0]*ED[0]-EP[0]*EP[0]);
//solve fT
fT_a[ith]=-TK_a[ith]*(EP[0]+ED[0])/(2*(ED[ith]+EP[ith]))/mom[ith];
fT_n[ith]=-TK_n[ith]*(EP[0]+ED[0])/(2*(ED[ith]+EP[ith]))/mom[ith];
}
//write analytic and umeric plot of fP and f0
ofstream out_fP_a("plots/fP_a.xmg"),out_fP_n("plots/fP_n.xmg");
ofstream out_fM_a("plots/fM_a.xmg"),out_fM_n("plots/fM_n.xmg");
ofstream out_f0_a("plots/f0_a.xmg"),out_f0_n("plots/f0_n.xmg");
ofstream out_fT_a("plots/fT_a.xmg"),out_fT_n("plots/fT_n.xmg");
out_fP_a<<"@type xydy"<<endl;
out_fP_n<<"@type xydy"<<endl;
out_f0_a<<"@type xydy"<<endl;
out_f0_n<<"@type xydy"<<endl;
out_fM_a<<"@type xydy"<<endl;
out_fM_n<<"@type xydy"<<endl;
out_fT_a<<"@type xydy"<<endl;
out_fT_n<<"@type xydy"<<endl;
out_f0_a<<Q2[0].med()<<" "<<f0_a[0]<<endl;
out_f0_n<<Q2[0].med()<<" "<<f0_n[0]<<endl;
for(int ith=1;ith<nth;ith++)
{
out_fP_a<<Q2[ith].med()<<" "<<fP_a[ith]<<endl;
out_fP_n<<Q2[ith].med()<<" "<<fP_n[ith]<<endl;
out_fM_a<<Q2[ith].med()<<" "<<fM_a[ith]<<endl;
out_fM_n<<Q2[ith].med()<<" "<<fM_n[ith]<<endl;
out_f0_a<<Q2[ith].med()<<" "<<f0_a[ith]<<endl;
out_f0_n<<Q2[ith].med()<<" "<<f0_n[ith]<<endl;
out_fT_a<<Q2[ith].med()<<" "<<fT_a[ith]<<endl;
out_fT_n<<Q2[ith].med()<<" "<<fT_n[ith]<<endl;
}
ext_EP=EP;
ext_ED=ED;
ext_Q2=Q2;
ext_fP=fP_a;
ext_fM=fM_a;
ext_f0=f0_a;
ext_fT=fT_a;
}
TEST(KalmanFilterTest, functionality)
{
KalmanFilter kf;
MatrixXd A(1,1); A << 1;
kf.setStateTransitionModel(A);
MatrixXd H(1,1); H << 1;
kf.setObservationModel(H);
MatrixXd Q(1,1); Q << 0.1;
kf.setProcessNoiseCovariance(Q);
MatrixXd R(1,1); R << 10;
kf.setMeasurementNoiseCovariance(R);
kf.validate();
// check if the calculation of an estimate is correct
InputValue measurement(1);
Input in(measurement);
Output out = kf.estimate(in);
EXPECT_EQ(out.size(), 1);
EXPECT_NEAR(out[0].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.0990, 0.0001);
in[0].setValue(5);
EXPECT_NO_THROW(out = kf.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1951, 0.0001);
// missing measurement
InputValue missingValue;
Input inMissing(missingValue);
EXPECT_NO_THROW(out = kf.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.2866, 0.0001);
// another example ---------------------------------
KalmanFilter kf2;
MatrixXd A2(2,2); A2 << 1,0.1,0,1;
kf2.setStateTransitionModel(A2);
MatrixXd H2(1,2); H2 << 0,1;
kf2.setObservationModel(H2);
MatrixXd Q2(2,2); Q2 << 0.1,0,0,0.1;
kf2.setProcessNoiseCovariance(Q2);
MatrixXd R2(1,1); R2 << 10;
kf2.setMeasurementNoiseCovariance(R2);
kf2.validate();
in[0].setValue(1);
EXPECT_NO_THROW(out = kf2.estimate(in));
EXPECT_EQ(out.size(), 2);
EXPECT_NEAR(out[0].getValue(), 0.0, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.0990, 0.0001);
// add control input
MatrixXd B2(2,1); B2 << 0,1;
kf2.setControlInputModel(B2); // needs to be validated
InputValue ctrl(0.5);
Input in_ctrl(ctrl);
kf2.setControlInput(in_ctrl);
in[0].setValue(5);
EXPECT_NO_THROW(kf2.validate());
EXPECT_NO_THROW(out = kf2.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.0053, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.20098, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.1951, 0.0001);
// pass control input with invalid size
in_ctrl.add(ctrl); // -> u.size = 2
// setting invalid control input throws exception
EXPECT_THROW(kf2.setControlInput(in_ctrl), length_error);
// changing control input model works
MatrixXd B3(2,2); B3 << 0,0,0,0;
EXPECT_NO_THROW(kf2.setControlInputModel(B3));
EXPECT_ANY_THROW(out = kf2.estimate(in)); // not validated
EXPECT_NO_THROW(kf2.validate());
// missing measurement
EXPECT_NO_THROW(out = kf2.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.0651, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.3048, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.5975, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.2867, 0.0001);
}
int main( int argc, char* argv[] ) {
printf("Test quaternions.\n");
{
printf("\n=== 1 =============================================\n");
CQuaternion Q1;
Dump(Q1);
}
{
printf("\n=== 2 =============================================\n");
CQuaternion Q2( CVector(0.0f,0.0f,666.0f), (float)CONST_PI );
Dump(Q2);
}
{
printf("\n=== 3 =============================================\n");
CQuaternion Q3( 1, 1, 1, 1 );
Dump(Q3);
printf("%f\n", Q3.Norm() );
printf("%f\n", Q3.Length() );
Q3.Normalize();
Dump(Q3);
}
{
printf("\n=== 4 =============================================\n");
CQuaternion Q4( 1, 1, 1, 1 );
Dump(Q4);
CQuaternion Q41 = !Q4;
Dump(Q41);
CQuaternion Q42 = -Q4;
Dump(Q42);
}
{
printf("\n=== 5 =============================================\n");
CQuaternion Q51( 1, 2, 3, 4 );
Dump(Q51);
CQuaternion Q52( 1, 2, 3, -4 );
Dump(Q52);
printf("%f\n", Q51^Q52 );
}
{
printf("\n=== 6 =============================================\n");
CQuaternion Q61( 1, 2, 3, 4 );
Dump(Q61);
CQuaternion Q62( 5, 6, 7, 8 );
Dump(Q62);
Dump( Q61 + Q62 );
Dump( Q61 - Q62 );
Dump( Q61 += Q62 );
Dump( Q61 -= Q62 );
}
{
printf("\n=== 7 =============================================\n");
CQuaternion Q71( 1, 2, 3, 4 );
Q71.Normalize();
Dump(Q71);
printf("%f\n",Q71.Length());
//CQuaternion Q72( 5, 6, 7, 8 );
//Q72.Normalize();
//Dump(Q72);
//Dump( Q71 * Q72 );
CQuaternion Q73 = -Q71;
Dump( Q73 );
printf("%f\n",Q73.Length());
CQuaternion Product = Q71 * Q73;
Dump( Product );
printf("%f\n",Product.Length());
Dump( Q71*=Q73 );
}
{
printf("\n=== 8 =============================================\n");
CQuaternion Q8( CVector(0.0f,0.0f,1.0f), (float)CONST_PI_2 );
//CQuaternion Q8( CVector(0,0,1), CONST_PI );
Dump(Q8);
CVector Src(1,0,0);
CVector Dst = RotateVectorByQuaternion( Src, Q8 );
printf("%f %f %f\n",Dst.x,Dst.y,Dst.z);
}
{
printf("\n=== 9 =============================================\n");
CQuaternion Q81( CVector(0,0,1), 0 );
Dump(Q81);
CQuaternion Q82( CVector(0,0,1), CONST_PI );
Dump(Q82);
for( float t=0.0f; t<1.0f; t+=0.1f ) {
CQuaternion R = SLerp(Q81,Q82,t);
Dump( R );
}
}
{
printf("\n=== 10 ============================================\n");
CQuaternion Q10( CVector(0,0,1), CONST_PI_2 );
//CQuaternion Q10( CVector(0,0,1), CONST_PI );
Dump(Q10);
CVector V;
float A;
Q10.ToAxisAngle( V, A );
printf("%f %f %f\n",V.x,V.y,V.z);
printf("%f\n",A);
CVector Src(1,0,0);
CVector Dst1 = RotateVectorByQuaternion( Src, Q10 );
printf("%f %f %f\n",Dst1.x,Dst1.y,Dst1.z);
CVector Dst11 = Src*Q10;
printf("%f %f %f\n",Dst11.x,Dst11.y,Dst11.z);
CMatrix M( Q10.ToMatrix() );
CVector Dst2 = Src*M;
printf("%f %f %f\n",Dst2.x,Dst2.y,Dst2.z);
CMatrix M3;
M3.ConstructRotation( CVector(0,0,1), CONST_PI_2 );
CVector Dst3 = Src*M3;
printf("%f %f %f\n",Dst3.x,Dst3.y,Dst3.z);
CMatrix M4;
M4.ConstructRotationZ( CONST_PI_2 );
CVector Dst4 = Src*M4;
printf("%f %f %f\n",Dst4.x,Dst4.y,Dst4.z);
}
{
printf("\n=== 11 =============================================\n");
CMatrix M11;
M11.ConstructRotation( CVector(0.3f,-0.78f,-0.9f), 0.666 );
CQuaternion Q11 = CreateNonUnitQuaternionFromRotationMatrix( M11 );
Q11.Normalize();
CQuaternion Q111 = CreateUnitQuaternionFromRotationMatrix( M11 );
Q111.Normalize();
CVector V11( 0.0f, 0.0f, 1.0f );
CVector R1 = V11*M11;
printf("%f %f %f\n",R1.x,R1.y,R1.z);
CVector R2 = RotateVectorByQuaternion( V11, Q11 );
printf("%f %f %f\n",R2.x,R2.y,R2.z);
CVector R3 = RotateVectorByQuaternion( V11, Q111 );
printf("%f %f %f\n",R3.x,R3.y,R3.z);
}
{
printf("\n=== 12 =============================================\n");
CQuaternion Q1( CVector(1.0f,1.0f,1.0f), (float)CONST_PI_3 );
Q1.Normalize();
CQuaternion Q2( CVector(1.0f,1.0f,1.0f), (float)CONST_PI_3 );
Q2.Normalize();
CQuaternion Q3 = SLerp(Q1,Q2,0.5f);
Dump(Q3);
}
}
int Colisao::colisaoJogadorDisco(int j)
{
static bool antcol[2]; // estado anterior da colisao (true/false)
// Seja P o jogador e Q o disco.
// Seja P1 a posicao anterior e P2 a posicao atual (o mesmo vale para Q)
Vetor P1(jog[j]->getOldPos()), P2(jog[j]->getPos());
Vetor Q1(disco->getOldPos()), Q2(disco->getPos());
// Calcularemos agora as velocidades
Vetor VP = P2 - P1;
Vetor VQ = Q2 - Q1;
// Definindo variaveis para otimizar o calculo
Vetor A = P1 - Q1;
Vetor B = VP - VQ;
float A2 = A.prodEscalar(A);
float B2 = B.prodEscalar(B);
float AB = A.prodEscalar(B);
float AB2 = AB * AB;
// quadrado da distancia entre os centros
float d2 = pow(jog[j]->getRaio() + disco->getRaio(), 2);
float t; // tempo
// se ainda nao esta colidindo
if ((jog[j]->getPos() - disco->getPos()).norma() > jog[j]->getRaio() + disco->getRaio()) {
//LD fixes
float zero = 0.0f;
// Teste para verificar a nao-colisao
if (B == zero) // movimento relativo == 0
{ antcol[j] = false; return 0; } // nao houve colisao
// Teste para verificar a nao-colisao
if (A2 - AB2 / B2 > d2)
{ antcol[j] = false; return 0; } // nao houve colisao
// Teste para verificar a nao colisao
float raiz = AB2 - B2 * (A2 - d2);
if (raiz < 0)
{ antcol[j] = false; return 0; } // nao houve colisao
// tempo da colisao
t = (-(AB) - sqrt(raiz)) / B2;
// Teste para verificar a nao-colisao
if (!(t > 0 && t <= 1))
{ antcol[j] = false; return 0; } // nao houve colisao
// Verifica se ja houve colisao no instante anterior
if (antcol[j])
return 0;
// Atualiza posicoes para o instante de colisao
jog[j]->setPos(P1 + t * VP);
disco->setPos(Q1 + t * VQ);
}
else {
// Atualiza a posicao do disco (para ficar _fora_ do jog[j]->
Vetor normal = (disco->getPos() - jog[j]->getPos()).versor() * (disco->getRaio() + jog[j]->getRaio());
disco->setPos(jog[j]->getPos() + normal);
}
// normal (do disco para o jogador)
Vetor normal(jog[j]->getPos() - disco->getPos());
normal = (disco->getVel().prodEscalar(normal) / normal.prodEscalar(normal)) * normal; // projecao
// vetor tangente
Vetor tangente = disco->getVel() - normal;
// velocidade resultante (da disco)
disco->setVel(tangente - normal);
// afasta um pouco o disco do jogador
disco->setPos(disco->getPos() + disco->getVel().versor() * 3);
// Aumenta velocidade do disco (armengue)
Vetor v = disco->getVel().versor();
if (v.norma() == 0)
v = jog[j]->getVel().versor() * 10.0;
disco->setVel(disco->getVel() + v * 2.0);
antcol[j] = true;
return 1;
}
//---------------------------------------------------------
void CurvedINS2D::InitRun()
//---------------------------------------------------------
{
// construct grid and metric
StartUp2D();
// Optional mesh refinement: split each parent
// element into 4 conforming "child" elements
if (Nrefine>0) {
umLOG(1, "before refine : K = %5d\n", K);
DMat Q2(Np*K, 1); IMat refineflag;
refineflag = Ones(K,Nfaces);
for (int i=1; i<=Nrefine; ++i) {
Q2 = ConformingHrefine2D(refineflag, Q2);
umLOG(1, "after refine %d: K = %5d\n", i, K);
}
}
// Adjust faces on circular boundaries,
// and perform any sim-specific setup:
switch (sim_type) {
case eVolkerCylinder:
// move Cylinder bdry faces to radius 0.05
AdjustCylBC(0.05, 0.,0., BC_Cyl);
break;
default:
// set default maps for {straight,curved} elements
straight.range(1,K); curved.resize(0);
break;
}
Resize(); // allocate arrays
BuildBCMaps2D(); // build boundary condition maps
SetIC(); // set initial conditions
SetStepSize(); // calculate step size (dt)
// reset various work timers
time_setup = time_advection = 0.0;
time_viscous = time_viscous_sol = 0.0;
time_pressure = time_pressure_sol = 0.0;
//---------------------------------------------
// base class version sets counters and flags
//---------------------------------------------
NDG2D::InitRun();
//---------------------------------------------
// set frequency of reporting
//---------------------------------------------
//Nreport = Nsteps/150;
//Nreport = 10;
//Nreport = 2;
Nreport = 10;
//Nreport = 250;
//Nreport = 1000;
//Nreport = 10000;
//---------------------------------------------
// set frequency of rendering
//---------------------------------------------
Nrender = Nreport;
//Nrender = 250;
//Nrender = 1000;
//NvtkInterp = 12; // set output resolution
//NvtkInterp = 5; // set output resolution
NvtkInterp = this->N; // use original nodes
Summary(); // show simulation details
}
/*! \fn CTU_Algorithm_2D(Real *C, int nx, int ny, int n_ghost, Real dx, Real dy, Real dt)
*! \brief The corner transport upwind algorithm of Gardiner & Stone, 2008. */
void CTU_Algorithm_2D(Real *C, int nx, int ny, int n_ghost, Real dx, Real dy, Real dt)
{
int n_cells = nx*ny;
// Create structures to hold the initial input states and associated interface fluxes (Q* and F* from Stone, 2008)
States_2D Q1(n_cells);
Fluxes_2D F1(n_cells);
// and the states and fluxes after half a timestep of evolution (Q n+1/2 and F n+1/2)
States_2D Q2(n_cells);
Fluxes_2D F2(n_cells);
// Create arrays to hold the eta values for the H correction
Real *eta_x = (Real *) malloc(n_cells*sizeof(Real));
Real *eta_y = (Real *) malloc(n_cells*sizeof(Real));
// Set H correction to zero
Real etah = 0.0;
// Set up pointers to the appropriate locations in C to read and modify values of conserved variables
Real *density = &C[0];
Real *momentum_x = &C[n_cells];
Real *momentum_y = &C[2*n_cells];
Real *momentum_z = &C[3*n_cells];
Real *Energy = &C[4*n_cells];
// Declare iteration variables
int i, j;
int istart, istop, jstart, jstop;
// index of each cell
int id;
Real dtodx = dt/dx;
Real dtody = dt/dy;
// Step 1: Calculate the left and right states at each cell interface
#ifdef PCM
// sweep through cells and use cell averages to set input states
// do the calculation for all interfaces
// the new left and right states for each i+1/2 interface are assigned to cell i
istart = 0; istop = nx-1;
jstart = 0; jstop = ny;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
id = i + j*nx;
Q1.d_Lx[i + j*nx] = density[id];
Q1.mx_Lx[i + j*nx] = momentum_x[id];
Q1.my_Lx[i + j*nx] = momentum_y[id];
Q1.mz_Lx[i + j*nx] = momentum_z[id];
Q1.E_Lx[i + j*nx] = Energy[id];
id = (i+1) + j*nx;
Q1.d_Rx[i + j*nx] = density[id];
Q1.mx_Rx[i + j*nx] = momentum_x[id];
Q1.my_Rx[i + j*nx] = momentum_y[id];
Q1.mz_Rx[i + j*nx] = momentum_z[id];
Q1.E_Rx[i + j*nx] = Energy[id];
}
}
istart = 0; istop = nx;
jstart = 0; jstop = ny-1;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
id = i + j*nx;
Q1.d_Ly[i + j*nx] = density[id];
Q1.mx_Ly[i + j*nx] = momentum_x[id];
Q1.my_Ly[i + j*nx] = momentum_y[id];
Q1.mz_Ly[i + j*nx] = momentum_z[id];
Q1.E_Ly[i + j*nx] = Energy[id];
id = i + (j+1)*nx;
Q1.d_Ry[i + j*nx] = density[id];
Q1.mx_Ry[i + j*nx] = momentum_x[id];
Q1.my_Ry[i + j*nx] = momentum_y[id];
Q1.mz_Ry[i + j*nx] = momentum_z[id];
Q1.E_Ry[i + j*nx] = Energy[id];
}
}
#endif //PCM
#if defined (PLMP) || defined (PLMC)
// sweep through cells, use the piecewise linear method to calculate reconstructed boundary values
// create the stencil of conserved variables needed to calculate the boundary values
// on either side of the cell interface
Real stencil[15];
// create array to hold the boundary values
// returned from the linear reconstruction function (conserved variables)
Real bounds[10];
// x direction
istart = n_ghost-1; istop = nx-n_ghost+1;
for (j=0; j<ny; j++) {
for (i=istart; i<istop; i++) {
// fill the stencil for the x-direction
id = i + j*nx;
stencil[0] = density[id];
stencil[1] = momentum_x[id];
stencil[2] = momentum_y[id];
stencil[3] = momentum_z[id];
stencil[4] = Energy[id];
id = (i-1) + j*nx;
stencil[5] = density[id];
stencil[6] = momentum_x[id];
stencil[7] = momentum_y[id];
stencil[8] = momentum_z[id];
stencil[9] = Energy[id];
id = (i+1) + j*nx;
stencil[10] = density[id];
stencil[11] = momentum_x[id];
stencil[12] = momentum_y[id];
stencil[13] = momentum_z[id];
stencil[14] = Energy[id];
// pass the stencil to the linear reconstruction function - returns the reconstructed left
// and right boundary values for the cell (conserved variables)
#ifdef PLMP
plmp(stencil, bounds, dx, dt, gama);
#endif
#ifdef PLMC
plmc(stencil, bounds, dx, dt, gama);
//lr_states(stencil, bounds, dx, dt, gama);
#endif
// place the boundary values in the relevant array
// the reconstruction function returns l & r for cell i, i.e. |L i R|
// for compatibility with ppm, switch this so that the input states for the i+1/2 interface
// are associated with the ith cell
id = (i-1) + j*nx;
Q1.d_Rx[id] = bounds[0];
Q1.mx_Rx[id] = bounds[1];
Q1.my_Rx[id] = bounds[2];
Q1.mz_Rx[id] = bounds[3];
Q1.E_Rx[id] = bounds[4];
id = i + j*nx;
Q1.d_Lx[id] = bounds[5];
Q1.mx_Lx[id] = bounds[6];
Q1.my_Lx[id] = bounds[7];
Q1.mz_Lx[id] = bounds[8];
Q1.E_Lx[id] = bounds[9];
// now L&R correspond to left and right of the interface, i.e. L | R
}
}
// y-direction
jstart = n_ghost-1; jstop = ny-n_ghost+1;
for (j=jstart; j<jstop; j++) {
for (i=0; i<nx; i++) {
// fill the stencil for the y direction
id = i + j*nx;
stencil[0] = density[id];
stencil[1] = momentum_y[id];
stencil[2] = momentum_z[id];
stencil[3] = momentum_x[id];
stencil[4] = Energy[id];
id = i + (j-1)*nx;
stencil[5] = density[id];
stencil[6] = momentum_y[id];
stencil[7] = momentum_z[id];
stencil[8] = momentum_x[id];
stencil[9] = Energy[id];
id = i + (j+1)*nx;
stencil[10] = density[id];
stencil[11] = momentum_y[id];
stencil[12] = momentum_z[id];
stencil[13] = momentum_x[id];
stencil[14] = Energy[id];
// pass the stencil to the linear reconstruction function - returns the reconstructed left
// and right boundary values for the cell (conserved variables)
#ifdef PLMP
plmp(stencil, bounds, dy, dt, gama);
#endif
#ifdef PLMC
plmc(stencil, bounds, dy, dt, gama);
#endif
// place the boundary values in the relevant array
id = i + (j-1)*nx;
Q1.d_Ry[id] = bounds[0];
Q1.my_Ry[id] = bounds[1];
Q1.mz_Ry[id] = bounds[2];
Q1.mx_Ry[id] = bounds[3];
Q1.E_Ry[id] = bounds[4];
id = i + j*nx;
Q1.d_Ly[id] = bounds[5];
Q1.my_Ly[id] = bounds[6];
Q1.mz_Ly[id] = bounds[7];
Q1.mx_Ly[id] = bounds[8];
Q1.E_Ly[id] = bounds[9];
}
}
#endif //PLM or PLM_ATHENA
#if defined (PPMP) || defined (PPMC)
// sweep through cells, use PPM to calculate reconstructed boundary values
// create the stencil of conserved variables needed to calculate the boundary values
// on either side of the cell interface
#ifdef PPMP
Real stencil[35];
#endif
#ifdef PPMC
Real stencil[25];
#endif
// create an array to hold the intermediate boundary values
// returned from the ppm function (left and right, for d, mx, my, mz, E)
Real bounds[10];
// x-direction
istart = n_ghost-2; istop = nx-n_ghost+2;
jstart = 0; jstop = ny;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// fill the stencil for the x-direction
id = i + j*nx;
stencil[0] = density[id];
stencil[1] = momentum_x[id];
stencil[2] = momentum_y[id];
stencil[3] = momentum_z[id];
stencil[4] = Energy[id];
id = (i-1) + j*nx;
stencil[5] = density[id];
stencil[6] = momentum_x[id];
stencil[7] = momentum_y[id];
stencil[8] = momentum_z[id];
stencil[9] = Energy[id];
id = (i+1) + j*nx;
stencil[10] = density[id];
stencil[11] = momentum_x[id];
stencil[12] = momentum_y[id];
stencil[13] = momentum_z[id];
stencil[14] = Energy[id];
id = (i-2) + j*nx;
stencil[15] = density[id];
stencil[16] = momentum_x[id];
stencil[17] = momentum_y[id];
stencil[18] = momentum_z[id];
stencil[19] = Energy[id];
id = (i+2) + j*nx;
stencil[20] = density[id];
stencil[21] = momentum_x[id];
stencil[22] = momentum_y[id];
stencil[23] = momentum_z[id];
stencil[24] = Energy[id];
#ifdef PPMP
id = (i-3) + j*nx;
stencil[25] = density[id];
stencil[26] = momentum_x[id];
stencil[27] = momentum_y[id];
stencil[28] = momentum_z[id];
stencil[29] = Energy[id];
id = (i+3) + j*nx;
stencil[30] = density[id];
stencil[31] = momentum_x[id];
stencil[32] = momentum_y[id];
stencil[33] = momentum_z[id];
stencil[34] = Energy[id];
#endif
// pass the stencil to the ppm reconstruction function - returns the reconstructed left
// and right boundary values (conserved variables)
#ifdef PPMP
ppmp(stencil, bounds, dx, dt, gama);
#endif
#ifdef PPMC
ppmc(stencil, bounds, dx, dt, gama);
#endif
// place the boundary values in the relevant array
// at this point, L&R correspond to left and right of the interface i.e. L|R
id = i-1 + j*nx;
Q1.d_Rx[id] = bounds[0];
Q1.mx_Rx[id] = bounds[1];
Q1.my_Rx[id] = bounds[2];
Q1.mz_Rx[id] = bounds[3];
Q1.E_Rx[id] = bounds[4];
id = i + j*nx;
Q1.d_Lx[id] = bounds[5];
Q1.mx_Lx[id] = bounds[6];
Q1.my_Lx[id] = bounds[7];
Q1.mz_Lx[id] = bounds[8];
Q1.E_Lx[id] = bounds[9];
}
}
// y-direction
istart = 0; istop = nx;
jstart = n_ghost-2; jstop = ny-n_ghost+2;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// fill the stencil for the y direction
id = i + j*nx;
stencil[0] = density[id];
stencil[1] = momentum_y[id];
stencil[2] = momentum_z[id];
stencil[3] = momentum_x[id];
stencil[4] = Energy[id];
id = i + (j-1)*nx;
stencil[5] = density[id];
stencil[6] = momentum_y[id];
stencil[7] = momentum_z[id];
stencil[8] = momentum_x[id];
stencil[9] = Energy[id];
id = i + (j+1)*nx;
stencil[10] = density[id];
stencil[11] = momentum_y[id];
stencil[12] = momentum_z[id];
stencil[13] = momentum_x[id];
stencil[14] = Energy[id];
id = i + (j-2)*nx;
stencil[15] = density[id];
stencil[16] = momentum_y[id];
stencil[17] = momentum_z[id];
stencil[18] = momentum_x[id];
stencil[19] = Energy[id];
id = i + (j+2)*nx;
stencil[20] = density[id];
stencil[21] = momentum_y[id];
stencil[22] = momentum_z[id];
stencil[23] = momentum_x[id];
stencil[24] = Energy[id];
#ifdef PPMP
id = i + (j-3)*nx;
stencil[25] = density[id];
stencil[26] = momentum_y[id];
stencil[27] = momentum_z[id];
stencil[28] = momentum_x[id];
stencil[29] = Energy[id];
id = i + (j+3)*nx;
stencil[30] = density[id];
stencil[31] = momentum_y[id];
stencil[32] = momentum_z[id];
stencil[33] = momentum_x[id];
stencil[34] = Energy[id];
#endif
// pass the stencil to the ppm reconstruction function - returns the reconstructed left
// and right boundary values (conserved variables)
#ifdef PPMP
ppmp(stencil, bounds, dy, dt, gama);
#endif
#ifdef PPMC
ppmc(stencil, bounds, dy, dt, gama);
#endif
// place the boundary values in the relevant array
id = i + (j-1)*nx;
Q1.d_Ry[id] = bounds[0];
Q1.my_Ry[id] = bounds[1];
Q1.mz_Ry[id] = bounds[2];
Q1.mx_Ry[id] = bounds[3];
Q1.E_Ry[id] = bounds[4];
id = i + j*nx;
Q1.d_Ly[id] = bounds[5];
Q1.my_Ly[id] = bounds[6];
Q1.mz_Ly[id] = bounds[7];
Q1.mx_Ly[id] = bounds[8];
Q1.E_Ly[id] = bounds[9];
}
}
#endif //PPMP or PPMC
// Step 2: Using the input states, compute the 1D fluxes at each interface.
// Only do this for interfaces touching real cells (start at i = n_ghost-1 since
// flux for the i+1/2 interface is stored by cell i)
// Create arrays to hold the input states for the Riemann solver and the returned fluxes
Real cW[10];
Real flux[5];
// Solve the Riemann problem at each x-interface
// do the calculation for all the real interfaces in the x direction
// and one ghost cell on either side in the y direction
istart = n_ghost-2; istop = nx-n_ghost+1;
jstart = n_ghost-2; jstop = ny-n_ghost+2;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// set input variables for the x interfaces
// exact Riemann solver takes conserved variables
id = i + j*nx;
cW[0] = Q1.d_Lx[id];
cW[1] = Q1.d_Rx[id];
cW[2] = Q1.mx_Lx[id];
cW[3] = Q1.mx_Rx[id];
cW[4] = Q1.my_Lx[id];
cW[5] = Q1.my_Rx[id];
cW[6] = Q1.mz_Lx[id];
cW[7] = Q1.mz_Rx[id];
cW[8] = Q1.E_Lx[id];
cW[9] = Q1.E_Rx[id];
// call a Riemann solver to evaluate fluxes at the cell interface
#ifdef EXACT
Calculate_Exact_Fluxes(cW, flux, gama);
#endif
#ifdef ROE
Calculate_Roe_Fluxes(cW, flux, gama, etah);
#endif
// update the fluxes in the x-direction
F1.dflux_x[id] = flux[0];
F1.xmflux_x[id] = flux[1];
F1.ymflux_x[id] = flux[2];
F1.zmflux_x[id] = flux[3];
F1.Eflux_x[id] = flux[4];
}
}
// Solve the Riemann problem at each y-interface
// do the calculation for all the real interfaces in the y direction
// and one ghost cell on either side in the x direction
istart = n_ghost-2; istop = nx-n_ghost+2;
jstart = n_ghost-2; jstop = ny-n_ghost+1;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// set input variables for the y interfaces
id = i + j*nx;
cW[0] = Q1.d_Ly[id];
cW[1] = Q1.d_Ry[id];
cW[2] = Q1.my_Ly[id];
cW[3] = Q1.my_Ry[id];
cW[4] = Q1.mz_Ly[id];
cW[5] = Q1.mz_Ry[id];
cW[6] = Q1.mx_Ly[id];
cW[7] = Q1.mx_Ry[id];
cW[8] = Q1.E_Ly[id];
cW[9] = Q1.E_Ry[id];
// call a Riemann solver to evaluate fluxes at the cell interface
#ifdef EXACT
Calculate_Exact_Fluxes(cW, flux, gama);
#endif
#ifdef ROE
Calculate_Roe_Fluxes(cW, flux, gama, etah);
#endif
// update the fluxes in the y-direction
F1.dflux_y[id] = flux[0];
F1.ymflux_y[id] = flux[1];
F1.zmflux_y[id] = flux[2];
F1.xmflux_y[id] = flux[3];
F1.Eflux_y[id] = flux[4];
}
}
// Step 3: Evolve the left and right states at each interface by dt/2 using transverse flux gradients
// Only do this for interfaces bordering real cells (start at i = n_ghost-1 since
// flux for the i+1/2 interface is stored by cell i)
// Evolve the x-interface states
// do the calculation for all the real interfaces in the x direction
istart = n_ghost-2; istop = nx-n_ghost+1;
jstart = n_ghost-1; jstop = ny-n_ghost+1;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// density
Q2.d_Lx[i+j*nx] = Q1.d_Lx[i+j*nx] + 0.5*dtody*(F1.dflux_y[ i +(j-1)*nx] - F1.dflux_y[ i +j*nx]);
Q2.d_Rx[i+j*nx] = Q1.d_Rx[i+j*nx] + 0.5*dtody*(F1.dflux_y[(i+1)+(j-1)*nx] - F1.dflux_y[(i+1)+j*nx]);
// x-momentum
Q2.mx_Lx[i+j*nx] = Q1.mx_Lx[i+j*nx] + 0.5*dtody*(F1.xmflux_y[ i +(j-1)*nx] - F1.xmflux_y[ i +j*nx]);
Q2.mx_Rx[i+j*nx] = Q1.mx_Rx[i+j*nx] + 0.5*dtody*(F1.xmflux_y[(i+1)+(j-1)*nx] - F1.xmflux_y[(i+1)+j*nx]);
// y-momentum
Q2.my_Lx[i+j*nx] = Q1.my_Lx[i+j*nx] + 0.5*dtody*(F1.ymflux_y[ i +(j-1)*nx] - F1.ymflux_y[ i +j*nx]);
Q2.my_Rx[i+j*nx] = Q1.my_Rx[i+j*nx] + 0.5*dtody*(F1.ymflux_y[(i+1)+(j-1)*nx] - F1.ymflux_y[(i+1)+j*nx]);
// z-momentum
Q2.mz_Lx[i+j*nx] = Q1.mz_Lx[i+j*nx] + 0.5*dtody*(F1.zmflux_y[ i +(j-1)*nx] - F1.zmflux_y[ i +j*nx]);
Q2.mz_Rx[i+j*nx] = Q1.mz_Rx[i+j*nx] + 0.5*dtody*(F1.zmflux_y[(i+1)+(j-1)*nx] - F1.zmflux_y[(i+1)+j*nx]);
// Energy
Q2.E_Lx[i+j*nx] = Q1.E_Lx[i+j*nx] + 0.5*dtody*(F1.Eflux_y[ i +(j-1)*nx] - F1.Eflux_y[ i +j*nx]);
Q2.E_Rx[i+j*nx] = Q1.E_Rx[i+j*nx] + 0.5*dtody*(F1.Eflux_y[(i+1)+(j-1)*nx] - F1.Eflux_y[(i+1)+j*nx]);
}
}
// Evolve the y-interface states
// do the calculation for all the real interfaces in the y direction
istart = n_ghost-1; istop = nx-n_ghost+1;
jstart = n_ghost-2; jstop = ny-n_ghost+1;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// density
Q2.d_Ly[i+j*nx] = Q1.d_Ly[i+j*nx] + 0.5*dtodx*(F1.dflux_x[(i-1)+ j *nx] - F1.dflux_x[i+ j *nx]);
Q2.d_Ry[i+j*nx] = Q1.d_Ry[i+j*nx] + 0.5*dtodx*(F1.dflux_x[(i-1)+(j+1)*nx] - F1.dflux_x[i+(j+1)*nx]);
// x-momentum
Q2.mx_Ly[i+j*nx] = Q1.mx_Ly[i+j*nx] + 0.5*dtodx*(F1.xmflux_x[(i-1)+ j *nx] - F1.xmflux_x[i+ j *nx]);
Q2.mx_Ry[i+j*nx] = Q1.mx_Ry[i+j*nx] + 0.5*dtodx*(F1.xmflux_x[(i-1)+(j+1)*nx] - F1.xmflux_x[i+(j+1)*nx]);
// y-momentum
Q2.my_Ly[i+j*nx] = Q1.my_Ly[i+j*nx] + 0.5*dtodx*(F1.ymflux_x[(i-1)+ j *nx] - F1.ymflux_x[i+ j *nx]);
Q2.my_Ry[i+j*nx] = Q1.my_Ry[i+j*nx] + 0.5*dtodx*(F1.ymflux_x[(i-1)+(j+1)*nx] - F1.ymflux_x[i+(j+1)*nx]);
// z-momentum
Q2.mz_Ly[i+j*nx] = Q1.mz_Ly[i+j*nx] + 0.5*dtodx*(F1.zmflux_x[(i-1)+ j *nx] - F1.zmflux_x[i+ j *nx]);
Q2.mz_Ry[i+j*nx] = Q1.mz_Ry[i+j*nx] + 0.5*dtodx*(F1.zmflux_x[(i-1)+(j+1)*nx] - F1.zmflux_x[i+(j+1)*nx]);
// Energy
Q2.E_Ly[i+j*nx] = Q1.E_Ly[i+j*nx] + 0.5*dtodx*(F1.Eflux_x[(i-1)+ j *nx] - F1.Eflux_x[i+ j *nx]);
Q2.E_Ry[i+j*nx] = Q1.E_Ry[i+j*nx] + 0.5*dtodx*(F1.Eflux_x[(i-1)+(j+1)*nx] - F1.Eflux_x[i+(j+1)*nx]);
}
}
#ifdef H_CORRECTION
// Step 3 1/2: Calculate the eta values for the H correction of Sanders et al., 1998
// do the calculation for all the real interfaces in the x direction
istart = n_ghost-2; istop = nx-n_ghost+1;
jstart = n_ghost-1; jstop = ny-n_ghost+1;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// set input variables for the x interfaces
id = i + j*nx;
cW[0] = Q2.d_Lx[id];
cW[1] = Q2.d_Rx[id];
cW[2] = Q2.mx_Lx[id];
cW[3] = Q2.mx_Rx[id];
cW[4] = Q2.my_Lx[id];
cW[5] = Q2.my_Rx[id];
cW[6] = Q2.mz_Lx[id];
cW[7] = Q2.mz_Rx[id];
cW[8] = Q2.E_Lx[id];
cW[9] = Q2.E_Rx[id];
eta_x[id] = calc_eta(cW, gama);
}
}
// do the calculation for all the real y interfaces
istart = n_ghost-1; istop = nx-n_ghost+1;
jstart = n_ghost-2; jstop = ny-n_ghost+1;
for (i=istart; i<istop; i++) {
for (j=jstart; j<jstop; j++) {
// set input variables for the y interfaces
id = i + j*nx;
cW[0] = Q2.d_Ly[id];
cW[1] = Q2.d_Ry[id];
cW[2] = Q2.my_Ly[id];
cW[3] = Q2.my_Ry[id];
cW[4] = Q2.mz_Ly[id];
cW[5] = Q2.mz_Ry[id];
cW[6] = Q2.mx_Ly[id];
cW[7] = Q2.mx_Ry[id];
cW[8] = Q2.E_Ly[id];
cW[9] = Q2.E_Ry[id];
eta_y[id] = calc_eta(cW, gama);
}
}
#endif // H_CORRECTION
// Step 4: Compute new fluxes at cell interfaces using the corrected left and right
// states from step 3.
// Again, only do this for interfaces bordering real cells (start at i = n_ghost-1 since
// flux for the i+1/2 interface is stored by cell i).
// Solve the Riemann problem at each x-interface
// do the calculation for all the real x interfaces
istart = n_ghost-1; istop = nx-n_ghost;
jstart = n_ghost-1; jstop = ny-n_ghost+1;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
// set input variables for the x interfaces
// exact Riemann solver takes conserved variables
id = i + j*nx;
cW[0] = Q2.d_Lx[id];
cW[1] = Q2.d_Rx[id];
cW[2] = Q2.mx_Lx[id];
cW[3] = Q2.mx_Rx[id];
cW[4] = Q2.my_Lx[id];
cW[5] = Q2.my_Rx[id];
cW[6] = Q2.mz_Lx[id];
cW[7] = Q2.mz_Rx[id];
cW[8] = Q2.E_Lx[id];
cW[9] = Q2.E_Rx[id];
#ifdef H_CORRECTION
etah = fmax(eta_y[i + (j-1)*nx], eta_y[i + j*nx]);
etah = fmax(etah, eta_x[id]);
etah = fmax(etah, eta_y[i+1 + (j-1)*nx]);
etah = fmax(etah, eta_y[i+1 + j*nx]);
#endif
// call a Riemann solver to evaluate fluxes at the cell interface
#ifdef EXACT
Calculate_Exact_Fluxes(cW, flux, gama);
#endif
#ifdef ROE
Calculate_Roe_Fluxes(cW, flux, gama, etah);
#endif
// update the fluxes in the x-direction
F2.dflux_x[id] = flux[0];
F2.xmflux_x[id] = flux[1];
F2.ymflux_x[id] = flux[2];
F2.zmflux_x[id] = flux[3];
F2.Eflux_x[id] = flux[4];
}
}
// Solve the Riemann problem at each y-interface
// do the calculation for all the real y interfaces
istart = n_ghost-1; istop = nx-n_ghost+1;
jstart = n_ghost-1; jstop = ny-n_ghost;
for (i=istart; i<istop; i++) {
for (j=jstart; j<jstop; j++) {
// set input variables for the y interfaces
id = i + j*nx;
cW[0] = Q2.d_Ly[id];
cW[1] = Q2.d_Ry[id];
cW[2] = Q2.my_Ly[id];
cW[3] = Q2.my_Ry[id];
cW[4] = Q2.mz_Ly[id];
cW[5] = Q2.mz_Ry[id];
cW[6] = Q2.mx_Ly[id];
cW[7] = Q2.mx_Ry[id];
cW[8] = Q2.E_Ly[id];
cW[9] = Q2.E_Ry[id];
#ifdef H_CORRECTION
etah = fmax(eta_x[i-1 + j*nx], eta_x[i + j*nx]);
etah = fmax(etah, eta_y[id]);
etah = fmax(etah, eta_x[i-1 + (j+1)*nx]);
etah = fmax(etah, eta_x[i + (j+1)*nx]);
#endif
// call a Riemann solver to evaluate fluxes at the cell interface
#ifdef EXACT
Calculate_Exact_Fluxes(cW, flux, gama);
#endif
#ifdef ROE
Calculate_Roe_Fluxes(cW, flux, gama, etah);
#endif
// update the fluxes in the y-direction
F2.dflux_y[id] = flux[0];
F2.ymflux_y[id] = flux[1];
F2.zmflux_y[id] = flux[2];
F2.xmflux_y[id] = flux[3];
F2.Eflux_y[id] = flux[4];
}
}
// Step 5: Update the solution from time level n to n+1
// Only update real cells
istart = n_ghost; istop = nx-n_ghost;
jstart = n_ghost; jstop = ny-n_ghost;
for (j=jstart; j<jstop; j++) {
for (i=istart; i<istop; i++) {
density[ i+j*nx] += dtodx*(F2.dflux_x[ (i-1)+j*nx] - F2.dflux_x[ i+j*nx]) + dtody*(F2.dflux_y[ i+(j-1)*nx] - F2.dflux_y[ i+j*nx]);
momentum_x[i+j*nx] += dtodx*(F2.xmflux_x[(i-1)+j*nx] - F2.xmflux_x[i+j*nx]) + dtody*(F2.xmflux_y[i+(j-1)*nx] - F2.xmflux_y[i+j*nx]);
momentum_y[i+j*nx] += dtodx*(F2.ymflux_x[(i-1)+j*nx] - F2.ymflux_x[i+j*nx]) + dtody*(F2.ymflux_y[i+(j-1)*nx] - F2.ymflux_y[i+j*nx]);
momentum_z[i+j*nx] += dtodx*(F2.zmflux_x[(i-1)+j*nx] - F2.zmflux_x[i+j*nx]) + dtody*(F2.zmflux_y[i+(j-1)*nx] - F2.zmflux_y[i+j*nx]);
Energy[ i+j*nx] += dtodx*(F2.Eflux_x[ (i-1)+j*nx] - F2.Eflux_x[ i+j*nx]) + dtody*(F2.Eflux_y[ i+(j-1)*nx] - F2.Eflux_y[ i+j*nx]);
}
}
// free the interface states and flux structures
free(Q1.d_Lx);
free(Q2.d_Lx);
free(F1.dflux_x);
free(F2.dflux_x);
free(eta_x);
free(eta_y);
}
TEST(ExtendedKalmanFilterTest, functionality)
{
ExtendedKalmanFilter ekf;
ekf.setStateTransitionModel(f);
ekf.setJacobianOfStateTransitionModel(df);
ekf.setObservationModel(h);
ekf.setJacobianOfObservationModel(dh);
MatrixXd Q(1,1); Q << 0.1;
ekf.setProcessNoiseCovariance(Q);
MatrixXd R(1,1); R << 10;
ekf.setMeasurementNoiseCovariance(R);
ekf.validate();
// check if the calculation of an estimate is correct
// first measurement z1:
InputValue measurement(1);
Input in(measurement);
Output out;
// u contained in a state transition formula, so an error will be
// thrown when u is not initialized with the correct size
//out = ekf.estimate(in); // fail assertion produced by Eigen (out-of-range error)
ekf.setControlInputSize(1);
ekf.validate();
EXPECT_NO_THROW(out = ekf.estimate(in));
EXPECT_EQ(out.size(), 1);
EXPECT_NEAR(out[0].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.0990, 0.0001);
// next measurement z2:
in[0].setValue(5);
// setting a control input = 0 does not change the result, it can
// be calculated as if there is no control input
InputValue ctrl(0);
Input in_ctrl(ctrl);
ekf.setControlInput(in_ctrl); // (no validation needed, cannot be validated)
EXPECT_NO_THROW(out = ekf.estimate(in));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1951, 0.0001);
// missing measurement
InputValue missingValue;
Input inMissing(missingValue);
EXPECT_NO_THROW(out = ekf.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.1073, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.2866, 0.0001);
// another example -----------------------------------
ExtendedKalmanFilter ekf2;
ekf2.setStateTransitionModel(f2);
ekf2.setJacobianOfStateTransitionModel(df2);
ekf2.setObservationModel(h2);
ekf2.setJacobianOfObservationModel(dh2);
MatrixXd Q2(2,2); Q2 << 0.1, 0, 0, 0.1;
ekf2.setProcessNoiseCovariance(Q2);
MatrixXd R2(1,1); R2 << 10;
ekf2.setMeasurementNoiseCovariance(R2);
ekf2.validate();
in[0].setValue(1);
EXPECT_NO_THROW(out = ekf2.estimate(in));
EXPECT_EQ(out.size(), 2);
EXPECT_NEAR(out[0].getValue(), 0.0, 0.0001);
EXPECT_NEAR(out[0].getVariance(), 0.1, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
EXPECT_NEAR(out[1].getVariance(), 0.0990, 0.0001);
// missing measurement
EXPECT_NO_THROW(out = ekf2.estimate(inMissing));
EXPECT_NEAR(out[0].getValue(), 0.00099, 0.0001);
EXPECT_NEAR(out[1].getValue(), 0.0099, 0.0001);
}
int main(int argc,char **argv){
// Print GPU properties
//print_properties();
// Files to print the result after the last time step
FILE *rho_file;
FILE *E_file;
rho_file = fopen("rho_final.txt", "w");
E_file = fopen("E_final.txt", "w");
// Construct initial condition for problem
ICsinus Config(-1.0, 1.0, -1.0, 1.0);
//ICsquare Config(0.5,0.5,gasGam);
// Set initial values for Configuration 1
/*
Config.set_rho(rhoConfig19);
Config.set_pressure(pressureConfig19);
Config.set_u(uConfig19);
Config.set_v(vConfig19);
*/
// Determining global border based on left over tiles (a little hack)
int globalPadding;
globalPadding = (nx+2*border+16)/16;
globalPadding = 16*globalPadding - (nx+2*border);
//printf("Globalpad: %i\n", globalPadding);
// Change border to add padding
//border = border + globalPadding/2;
// Initiate the matrices for the unknowns in the Euler equations
cpu_ptr_2D rho(nx, ny, border,1);
cpu_ptr_2D E(nx, ny, border,1);
cpu_ptr_2D rho_u(nx, ny, border,1);
cpu_ptr_2D rho_v(nx, ny, border,1);
cpu_ptr_2D zeros(nx, ny, border,1);
// Set initial condition
Config.setIC(rho, rho_u, rho_v, E);
double timeStart = get_wall_time();
// Test
cpu_ptr_2D rho_dummy(nx, ny, border);
cpu_ptr_2D E_dummy(nx, ny, border);
/*
rho_dummy.xmin = -1.0;
rho_dummy.ymin = -1.0;
E_dummy.xmin = -1.0;
E_dummy.ymin = -1.0;
*/
// Set block and grid sizes
dim3 gridBC = dim3(1, 1, 1);
dim3 blockBC = dim3(BLOCKDIM_BC,1,1);
dim3 gridBlockFlux;
dim3 threadBlockFlux;
dim3 gridBlockRK;
dim3 threadBlockRK;
computeGridBlock(gridBlockFlux, threadBlockFlux, nx + 2*border, ny + 2*border, INNERTILEDIM_X, INNERTILEDIM_Y, BLOCKDIM_X, BLOCKDIM_Y);
computeGridBlock(gridBlockRK, threadBlockRK, nx + 2*border, ny + 2*border, BLOCKDIM_X_RK, BLOCKDIM_Y_RK, BLOCKDIM_X_RK, BLOCKDIM_Y_RK);
int nElements = gridBlockFlux.x*gridBlockFlux.y;
// Allocate memory for the GPU pointers
gpu_ptr_1D L_device(nElements);
gpu_ptr_1D dt_device(1);
gpu_ptr_2D rho_device(nx, ny, border);
gpu_ptr_2D E_device(nx, ny, border);
gpu_ptr_2D rho_u_device(nx, ny, border);
gpu_ptr_2D rho_v_device(nx, ny, border);
gpu_ptr_2D R0(nx, ny, border);
gpu_ptr_2D R1(nx, ny, border);
gpu_ptr_2D R2(nx, ny, border);
gpu_ptr_2D R3(nx, ny, border);
gpu_ptr_2D Q0(nx, ny, border);
gpu_ptr_2D Q1(nx, ny, border);
gpu_ptr_2D Q2(nx, ny, border);
gpu_ptr_2D Q3(nx, ny, border);
// Allocate pinned memory on host
init_allocate();
// Set BC arguments
set_bc_args(BCArgs[0], rho_device.getRawPtr(), rho_u_device.getRawPtr(), rho_v_device.getRawPtr(), E_device.getRawPtr(), nx+2*border, ny+2*border, border);
set_bc_args(BCArgs[1], Q0.getRawPtr(), Q1.getRawPtr(), Q2.getRawPtr(), Q3.getRawPtr(), nx+2*border, ny+2*border, border);
set_bc_args(BCArgs[2], rho_device.getRawPtr(), rho_u_device.getRawPtr(), rho_v_device.getRawPtr(), E_device.getRawPtr(), nx+2*border, ny+2*border, border);
// Set FLUX arguments
set_flux_args(fluxArgs[0], L_device.getRawPtr(), rho_device.getRawPtr(), rho_u_device.getRawPtr(), rho_v_device.getRawPtr(), E_device.getRawPtr(), R0.getRawPtr(),R1.getRawPtr(), R2.getRawPtr(), R3.getRawPtr(), nx, ny, border, rho.get_dx(), rho.get_dy(), theta, gasGam, INNERTILEDIM_X, INNERTILEDIM_Y);
set_flux_args(fluxArgs[1], L_device.getRawPtr(), Q0.getRawPtr(), Q1.getRawPtr(), Q2.getRawPtr(), Q3.getRawPtr(), R0.getRawPtr(),R1.getRawPtr(), R2.getRawPtr(), R3.getRawPtr(), nx, ny, border, rho.get_dx(), rho.get_dy(), theta, gasGam, INNERTILEDIM_X, INNERTILEDIM_Y);
// Set TIME argument
set_dt_args(dtArgs, L_device.getRawPtr(), dt_device.getRawPtr(), nElements, rho.get_dx(), rho.get_dy(), cfl_number);
// Set Rk arguments
set_rk_args(RKArgs[0], dt_device.getRawPtr(), rho_device.getRawPtr(), rho_u_device.getRawPtr(), rho_v_device.getRawPtr(), E_device.getRawPtr(), R0.getRawPtr(), R1.getRawPtr(), R2.getRawPtr(), R3.getRawPtr(), Q0.getRawPtr(), Q1.getRawPtr(), Q2.getRawPtr(), Q3.getRawPtr(), nx, ny, border);
set_rk_args(RKArgs[1], dt_device.getRawPtr(), Q0.getRawPtr(), Q1.getRawPtr(), Q2.getRawPtr(), Q3.getRawPtr(), R0.getRawPtr(), R1.getRawPtr(), R2.getRawPtr(), R3.getRawPtr(), rho_device.getRawPtr(), rho_u_device.getRawPtr(), rho_v_device.getRawPtr(), E_device.getRawPtr(), nx, ny, border);
L_device.set(FLT_MAX);
/*
R0.upload(zeros.get_ptr());
R1.upload(zeros.get_ptr());
R2.upload(zeros.get_ptr());
R3.upload(zeros.get_ptr());
Q0.upload(zeros.get_ptr());
Q1.upload(zeros.get_ptr());
Q2.upload(zeros.get_ptr());
Q3.upload(zeros.get_ptr());
*/
R0.set(0,0,0,nx,ny,border);
R1.set(0,0,0,nx,ny,border);
R2.set(0,0,0,nx,ny,border);
R3.set(0,0,0,nx,ny,border);
Q0.set(0,0,0,nx,ny,border);
Q1.set(0,0,0,nx,ny,border);
Q2.set(0,0,0,nx,ny,border);
Q3.set(0,0,0,nx,ny,border);
rho_device.upload(rho.get_ptr());
rho_u_device.upload(rho_u.get_ptr());
rho_v_device.upload(rho_v.get_ptr());
E_device.upload(E.get_ptr());
// Update boudries
callCollectiveSetBCPeriodic(gridBC, blockBC, BCArgs[0]);
//Create cuda stream
cudaStream_t stream1;
cudaStreamCreate(&stream1);
cudaEvent_t dt_complete;
cudaEventCreate(&dt_complete);
while (currentTime < timeLength && step < maxStep){
//RK1
//Compute flux
callFluxKernel(gridBlockFlux, threadBlockFlux, 0, fluxArgs[0]);
// Compute timestep (based on CFL condition)
callDtKernel(TIMETHREADS, dtArgs);
cudaMemcpyAsync(dt_host, dt_device.getRawPtr(), sizeof(float), cudaMemcpyDeviceToHost, stream1);
cudaEventRecord(dt_complete, stream1);
// Perform RK1 step
callRKKernel(gridBlockRK, threadBlockRK, 0, RKArgs[0]);
//Update boudries
callCollectiveSetBCPeriodic(gridBC, blockBC, BCArgs[1]);
//RK2
// Compute flux
callFluxKernel(gridBlockFlux, threadBlockFlux, 1, fluxArgs[1]);
//Perform RK2 step
callRKKernel(gridBlockRK, threadBlockRK, 1, RKArgs[1]);
//cudaEventRecord(srteam_sync, srteam1);
callCollectiveSetBCPeriodic(gridBC, blockBC, BCArgs[2]);
cudaEventSynchronize(dt_complete);
step++;
currentTime += *dt_host;
// printf("Step: %i, current time: %.6f dt:%.6f\n" , step,currentTime, dt_host[0]);
}
//cuProfilerStop();
//cudaProfilerStop();
printf("Elapsed time %.5f", get_wall_time() - timeStart);
E_device.download(E.get_ptr());
rho_u_device.download(rho_u.get_ptr());
rho_v_device.download(rho_v.get_ptr());
rho_device.download(rho_dummy.get_ptr());
rho_dummy.printToFile(rho_file, true, false);
Config.exactSolution(E_dummy, currentTime);
E_dummy.printToFile(E_file, true, false);
float LinfError = Linf(E_dummy, rho_dummy);
float L1Error = L1(E_dummy, rho_dummy);
float L1Error2 = L1test(E_dummy, rho_dummy);
printf("nx: %i\t Linf error %.9f\t L1 error %.7f L1test erro %.7f", nx, LinfError, L1Error, L1Error2);
printf("nx: %i step: %i, current time: %.6f dt:%.6f\n" , nx, step,currentTime, dt_host[0]);
/*
cudaMemcpy(L_host, L_device, sizeof(float)*(nElements), cudaMemcpyDeviceToHost);
for (int i =0; i < nElements; i++)
printf(" %.7f ", L_host[i]);
*/
printf("%s\n", cudaGetErrorString(cudaGetLastError()));
return(0);
}
