int cg(MAT &A,VEC b,VEC &x,int maxIter, double tol)
{
//A_p: A * p
VEC A_p(A.dim());
int node = std::sqrt(A.dim());
//p: search direction;
VEC p(A.dim()), r(A.dim()), newr(A.dim()), newx(A.dim());//,ans(A.dim()),temp(A.dim());
//MAT LU(A.dim()),a = A;
//r2: rT * r
double alpha, beta, r2, newr2, err;//,g;
//g = (double)(node-1)/2000.0;
int iter = 0;//
/*
LU = luFact(a); //LU in-place decomposition
temp=fwdSubs(LU,b); //forward substitution
ans= bckSubs(LU,temp);//backward substitution
*/
//Initial condition for CGM
p = r = b - (A * x);
r2 = r * r;
while(iter < maxIter)
{
A_p = A * p;
alpha = r2 / (p * A_p);
newx = x + (alpha * p);
err = std::sqrt(r2/A.dim());
if ( err < tol )
break;
newr = r - (alpha * A_p);
newr2 = newr * newr;
beta = newr2 / r2;
p = newr + beta * p;
//Re-initialization for next iteration
x = newx;
r = newr;
r2 = newr2;
//////////////////////////////////////
iter++;
}
//printf("cg:Vne: %lf; Vsw: %lf; Vse: %lf; R:%lf\n",newx[node-1],newx[(node-1)*node],newx[node*node-1], std::abs( 1/(g*(newx[0]-newx[1])+g*(newx[0]-newx[node] )) ) );
//printf("LU:Vne: %lf; Vsw: %lf; Vse: %lf; R:%lf\n",ans[node-1],ans[(node-1)*node],ans[node*node-1], std::abs( 1/(g*(ans[0]-ans[1])+g*(ans[0]-ans[node] )) ) );
//printf("Difference w.r.t. hw4: %E\n",linfnorm(ans - newx));
return iter;
}
unsigned long CSysSolve::CG_LinSolver(const CSysVector & b, CSysVector & x, CMatrixVectorProduct & mat_vec,
CPreconditioner & precond, su2double tol, unsigned long m, bool monitoring) {
int rank = 0;
#ifdef HAVE_MPI
MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#endif
/*--- Check the subspace size ---*/
if (m < 1) {
if (rank == MASTER_NODE) cerr << "CSysSolve::ConjugateGradient: illegal value for subspace size, m = " << m << endl;
#ifndef HAVE_MPI
exit(EXIT_FAILURE);
#else
MPI_Abort(MPI_COMM_WORLD,1);
MPI_Finalize();
#endif
}
CSysVector r(b);
CSysVector A_p(b);
/*--- Calculate the initial residual, compute norm, and check if system is already solved ---*/
mat_vec(x, A_p);
r -= A_p; // recall, r holds b initially
su2double norm_r = r.norm();
su2double norm0 = b.norm();
if ( (norm_r < tol*norm0) || (norm_r < eps) ) {
if (rank == MASTER_NODE) cout << "CSysSolve::ConjugateGradient(): system solved by initial guess." << endl;
return 0;
}
su2double alpha, beta, r_dot_z;
CSysVector z(r);
precond(r, z);
CSysVector p(z);
/*--- Set the norm to the initial initial residual value ---*/
norm0 = norm_r;
/*--- Output header information including initial residual ---*/
int i = 0;
if ((monitoring) && (rank == MASTER_NODE)) {
WriteHeader("CG", tol, norm_r);
WriteHistory(i, norm_r, norm0);
}
/*--- Loop over all search directions ---*/
for (i = 0; i < (int)m; i++) {
/*--- Apply matrix to p to build Krylov subspace ---*/
mat_vec(p, A_p);
/*--- Calculate step-length alpha ---*/
r_dot_z = dotProd(r, z);
alpha = dotProd(A_p, p);
alpha = r_dot_z / alpha;
/*--- Update solution and residual: ---*/
x.Plus_AX(alpha, p);
r.Plus_AX(-alpha, A_p);
/*--- Check if solution has converged, else output the relative residual if necessary ---*/
norm_r = r.norm();
if (norm_r < tol*norm0) break;
if (((monitoring) && (rank == MASTER_NODE)) && ((i+1) % 5 == 0)) WriteHistory(i+1, norm_r, norm0);
precond(r, z);
/*--- Calculate Gram-Schmidt coefficient beta,
beta = dotProd(r_{i+1}, z_{i+1}) / dotProd(r_{i}, z_{i}) ---*/
beta = 1.0 / r_dot_z;
r_dot_z = dotProd(r, z);
beta *= r_dot_z;
/*--- Gram-Schmidt orthogonalization; p = beta *p + z ---*/
p.Equals_AX_Plus_BY(beta, p, 1.0, z);
}
if ((monitoring) && (rank == MASTER_NODE)) {
cout << "# Conjugate Gradient final (true) residual:" << endl;
cout << "# Iteration = " << i << ": |res|/|res0| = " << norm_r/norm0 << ".\n" << endl;
}
// /*--- Recalculate final residual (this should be optional) ---*/
// mat_vec(x, A_p);
// r = b;
// r -= A_p;
// su2double true_res = r.norm();
//
// if (fabs(true_res - norm_r) > tol*10.0) {
// if (rank == MASTER_NODE) {
// cout << "# WARNING in CSysSolve::ConjugateGradient(): " << endl;
// cout << "# true residual norm and calculated residual norm do not agree." << endl;
// cout << "# true_res - calc_res = " << true_res - norm_r << endl;
// }
// }
return i;
}
void NavEstimator::prediction(float Ts)
{
float psidot, tmp, Vgdot;
if(fabsf(xhat_p(2)) < 0.01f)
{
xhat_p(2) = 0.01;
}
for(int i=0;i<N_;i++)
{
psidot = (qhat*sinf(phihat) + rhat*cosf(phihat))/cosf(thetahat);
tmp = -psidot*Vwhat*(xhat_p(4)*cosf(xhat_p(6)) + xhat_p(5)*sinf(xhat_p(6)))/xhat_p(2);
Vgdot = ((Vwhat*cosf(xhat_p(6)) + xhat_p(4))*(-psidot*Vwhat*sinf(xhat_p(6))) + (Vwhat*sinf(xhat_p(6)) + xhat_p(5))*(psidot*Vwhat*cosf(xhat_p(6))))/xhat_p(2);
f_p = Eigen::VectorXf::Zero(7);
f_p(0) = xhat_p(2)*cosf(xhat_p(3));
f_p(1) = xhat_p(2)*sinf(xhat_p(3));
f_p(2) = Vgdot;
f_p(3) = GRAVITY/xhat_p(2)*tanf(phihat)*cosf(xhat_p(3) - xhat_p(6));
f_p(6) = psidot;
A_p = Eigen::MatrixXf::Zero(7,7);
A_p(0,2) = cos(xhat_p(3));
A_p(0,3) = -xhat_p(2)*sinf(xhat_p(3));
A_p(1,2) = sin(xhat_p(3));
A_p(1,3) = xhat_p(2)*cosf(xhat_p(3));
A_p(2,2) = -Vgdot/xhat_p(2);
A_p(2,4) = -psidot*Vwhat*sinf(xhat_p(6))/xhat_p(2);
A_p(2,5) = psidot*Vwhat*cosf(xhat_p(6))/xhat_p(2);
A_p(2,6) = tmp;
A_p(3,2) = -GRAVITY/powf(xhat_p(2),2)*tanf(phihat)*cosf(xhat_p(3) - xhat_p(6));
A_p(3,3) = -GRAVITY/xhat_p(2)*tanf(phihat)*sinf(xhat_p(3) - xhat_p(6));
A_p(3,6) = GRAVITY/xhat_p(2)*tanf(phihat)*sinf(xhat_p(3) - xhat_p(6));
xhat_p += f_p *(Ts/N_);
P_p += (A_p*P_p + P_p*A_p.transpose() + Q_p)*(Ts/N_);
}
}
