void LinearizedKF::Update(const ompl::base::State *belief, const typename ObservationModelMethod::ObservationType& obs,
const LinearSystem& ls, ompl::base::State *updatedState)
{
using namespace arma;
colvec innov = this->observationModel_->computeInnovation(belief, obs);
if(!innov.n_rows || !innov.n_cols)
{
updatedState = si_->cloneState(belief);
return; // return the prediction if you don't have any innovation
}
assert(innov.n_rows);
assert(innov.n_cols);
mat covPred = belief->as<StateType>()->getCovariance();
mat rightMatrix = ls.getH() * covPred * trans(ls.getH()) + ls.getR();
mat leftMatrix = covPred * trans(ls.getH());
mat KalmanGain = solve(trans(rightMatrix), trans(leftMatrix));
KalmanGain = KalmanGain.t();
colvec xPredVec = belief->as<StateType>()->getArmaData();
colvec xEstVec = xPredVec + KalmanGain*innov;
updatedState->as<StateType>()->setXYYaw(xEstVec[0], xEstVec[1], xEstVec[2]);
mat covEst = covPred - KalmanGain* ls.getH() * covPred;
updatedState->as<StateType>()->setCovariance(covEst);
}
arma::mat LinearizedKF::computeStationaryCovariance (const LinearSystem& ls)
{
using namespace arma;
mat H = ls.getH();
mat M = ls.getM();
mat R = ls.getR();
mat Pprd;
bool dareSolvable = dare (trans(ls.getA()),trans(ls.getH()),ls.getG() * ls.getQ() * trans(ls.getG()),
ls.getM() * ls.getR() * trans(ls.getM()), Pprd );
if(!dareSolvable)
{
OMPL_ERROR("Dare Unsolvable: The given system state is not stable/observable. Try a different state.");
exit(1);
}
//makes this symmetric
Pprd = (Pprd + trans(Pprd)) / 2;
mat Pest = Pprd - ( Pprd * H.t()) * inv( H*Pprd*H.t() + M * R * M.t(), true) * trans(Pprd * H.t()) ;
//makes this symmetric
Pest = (Pest + trans(Pest)) / 2;
return Pest;
}
virtual void onIterationStart() {
// Only need to do this once
rA = right->A( nextTime() );
rb = right->b( nextTime() );
lA = left->A( nextTime() );
lb = left->b( nextTime() );
// Evaluate the predicate using the previous iterand
Vector predicate = rA * iterand(0) - rb;
Vector compare = domain->zero();
if(direct) {
compare = lA * iterand(0) - lb;
}
// Initialize penalty matrix
P.setZero();
P.reserve( IntegerVector::Constant( domain->size(), 1 ) );
// Initialize other matrix
Q.setZero();
Q.reserve( IntegerVector::Constant( domain->size(), 1 ) );
// Build penalty matrix
for(Index i = 0; i < domain->size(); ++i) {
const bool c = Order()(
(scale * predicate(i)),
compare(i)
);
if(c) { P.insert(i, i) = scale; }
if(!direct || !c) { Q.insert(i, i) = 1.; }
}
}
void LinearizedKF::Predict(const ompl::base::State *belief,
const ompl::control::Control* control, const LinearSystem& ls, ompl::base::State *predictedState)
{
using namespace arma;
this->motionModel_->Evolve(belief, control,this->motionModel_->getZeroNoise(), predictedState);
mat covPred = ls.getA() * belief->as<StateType>()->getCovariance() * trans(ls.getA()) +
ls.getG() * ls.getQ() * trans(ls.getG());
predictedState->as<StateType>()->setCovariance(covPred);
}
void
SpikeSolver::doSweeps(LinearSystem& ls, const int nSweeps)
{
const MultiFieldMatrix& m = ls.getMatrix();
MultiField& delta = ls.getDelta();
const MultiField& b = ls.getB();
MultiField& r = ls.getResidual();
for(int i=0; i<nSweeps; i++)
{
m.spikeSolve(delta,b,r,_spikeStorage);
}
}
MFRPtr
SpikeSolver::solve(LinearSystem & ls)
{
ls.getMatrix().computeResidual(ls.getDelta(),
ls.getB(),
ls.getResidual());
MFRPtr rNorm0(ls.getResidual().getOneNorm());
#ifdef FVM_PARALLEL
if (verbosity >0 && MPI::COMM_WORLD.Get_rank() == 0 )
cout << "0: " << *rNorm0 << "procID = " << MPI::COMM_WORLD.Get_rank() << endl;
#endif
#ifndef FVM_PARALLEL
if ( verbosity > 0 )
cout << "0: " << *rNorm0 << endl;
#endif
if (*rNorm0 < absoluteTolerance )
return rNorm0;
for(int i=1; i<nMaxIterations; i++)
{
doSweeps(ls,1);
ls.getMatrix().computeResidual(ls.getDelta(),
ls.getB(),
ls.getResidual());
MFRPtr rNorm(ls.getResidual().getOneNorm());
MFRPtr normRatio((*rNorm)/(*rNorm0));
#ifndef FVM_PARALLEL
if (verbosity >0 )
cout << i << ": " << *rNorm << endl;
#endif
#ifdef FVM_PARALLEL
if (*rNorm < absoluteTolerance || *normRatio < relativeTolerance || i == nMaxIterations-1)
if (verbosity >0 && MPI::COMM_WORLD.Get_rank() == 0 )
cout <<i << ": " << "procID = " << MPI::COMM_WORLD.Get_rank() << *rNorm << endl;
#endif
if (*rNorm < absoluteTolerance || *normRatio < relativeTolerance)
break;
}
return rNorm0;
}
int main() {
cout << "Linear equation systems. Gauss method.\nPodkopaev Anton, 345 group\n";
CurrentType coefArr[] = {
6.5126e-06, -8.0648e-03, 4.23528,
5.9176e-03, -0.80648, 1.46528,
0.87176, 0.79352, 0.91528
};
/*
CurrentType coefArr[] = {
1, 0.1, 0,
0, 1, 0,
0, 0, 1
};
*/
Matrix<CurrentType> systemCoef(3, 3, coefArr);
CurrentType constTermArr[] = {
3.61628,
1.52097,
1.81150
};
/*
CurrentType constTermArr[] = {
1,
2,
3
};
*/
Matrix<CurrentType> constTerm(3, 1, constTermArr);
LinearSystem<CurrentType>* sys = LinearSystem<CurrentType>::createSystem(systemCoef, constTerm);
//cout << (*sys);
//cout << endl << endl;
sys->gaussMethod();
sys->gaussMethodWithChoosingInLine();
if (sys)
delete sys;
return 0;
}
void TestChebyshevVsCG() throw (Exception)
{
unsigned num_nodes = 1331;
DistributedVectorFactory factory(num_nodes);
Vec parallel_layout = factory.CreateVec(2);
unsigned cg_its;
unsigned chebyshev_its;
Timer::Reset();
{
Mat system_matrix;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_matrix, "linalg/test/data/matrices/cube_6000elems_half_activated.mat", parallel_layout);
Vec system_rhs;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_rhs, "linalg/test/data/matrices/cube_6000elems_half_activated.vec", parallel_layout);
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetMatrixIsSymmetric();
ls.SetAbsoluteTolerance(1e-9);
ls.SetKspType("cg");
ls.SetPcType("bjacobi");
Vec solution = ls.Solve();
cg_its = ls.GetNumIterations();
PetscTools::Destroy(system_matrix);
PetscTools::Destroy(system_rhs);
PetscTools::Destroy(solution);
}
Timer::PrintAndReset("CG");
{
Mat system_matrix;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_matrix, "linalg/test/data/matrices/cube_6000elems_half_activated.mat", parallel_layout);
Vec system_rhs;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_rhs, "linalg/test/data/matrices/cube_6000elems_half_activated.vec", parallel_layout);
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetMatrixIsSymmetric();
ls.SetAbsoluteTolerance(1e-9);
ls.SetKspType("chebychev");
ls.SetPcType("bjacobi");
Vec solution = ls.Solve();
chebyshev_its = ls.GetNumIterations();
PetscTools::Destroy(system_matrix);
PetscTools::Destroy(system_rhs);
PetscTools::Destroy(solution);
}
Timer::Print("Chebyshev");
TS_ASSERT_LESS_THAN(cg_its, 15u); // Takes 14 iterations with 16 cores
TS_ASSERT_LESS_THAN(chebyshev_its, 17u); // Takes 16 iterations with 16 cores
PetscTools::Destroy(parallel_layout);
}
void TestChebyshevAdaptiveVsNoAdaptive() throw (Exception)
{
unsigned num_nodes = 1331;
DistributedVectorFactory factory(num_nodes);
Vec parallel_layout = factory.CreateVec(2);
// Solving with zero guess for coverage
Vec zero_guess = factory.CreateVec(2);
double zero = 0.0;
#if (PETSC_VERSION_MAJOR == 2 && PETSC_VERSION_MINOR == 2)
VecSet(&zero, zero_guess);
#else
VecSet(zero_guess, zero);
#endif
Mat system_matrix;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_matrix, "linalg/test/data/matrices/cube_6000elems_half_activated.mat", parallel_layout);
Vec system_rhs;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_rhs, "linalg/test/data/matrices/cube_6000elems_half_activated.vec", parallel_layout);
// Make sure we are not inheriting a non-default number of iterations from previous test
std::stringstream num_it_str;
num_it_str << 1000;
PetscOptionsSetValue("-ksp_max_it", num_it_str.str().c_str());
try
{
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetMatrixIsSymmetric();
// Solve to relative convergence for coverage
ls.SetRelativeTolerance(1e-6);
ls.SetPcType("jacobi");
ls.SetKspType("chebychev");
ls.SetUseFixedNumberIterations(true, 64);
// Solving with zero guess for coverage.
Vec solution = ls.Solve(zero_guess);
unsigned chebyshev_adaptive_its = ls.GetNumIterations();
TS_ASSERT_EQUALS(chebyshev_adaptive_its, 40u);
TS_ASSERT_DELTA(ls.mEigMin, 0.0124, 1e-4);
TS_ASSERT_DELTA(ls.mEigMax, 1.8810, 1e-4);
PetscTools::Destroy(solution);
}
catch (Exception& e)
{
if (e.GetShortMessage() == "Chebyshev with fixed number of iterations is known to be broken in PETSc <= 2.3.2")
{
WARNING(e.GetShortMessage());
}
else
{
TS_FAIL(e.GetShortMessage());
}
}
// Make sure we are not inheriting a non-default number of iterations from previous test
PetscOptionsSetValue("-ksp_max_it", num_it_str.str().c_str());
{
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetMatrixIsSymmetric();
ls.SetRelativeTolerance(1e-6);
ls.SetPcType("jacobi");
ls.SetKspType("chebychev");
Vec solution = ls.Solve(zero_guess);
unsigned chebyshev_no_adaptive_its = ls.GetNumIterations();
TS_ASSERT_LESS_THAN(chebyshev_no_adaptive_its, 100u); // Normally 88, but 99 on maverick & natty
TS_ASSERT_DELTA(ls.mEigMin, 0.0124, 1e-4);
TS_ASSERT_DELTA(ls.mEigMax, 1.8841, 1e-4);
PetscTools::Destroy(solution);
}
// Make sure we are not inheriting a non-default number of iterations from previous test
PetscOptionsSetValue("-ksp_max_it", num_it_str.str().c_str());
{
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetMatrixIsSymmetric();
ls.SetRelativeTolerance(1e-6);
ls.SetPcType("jacobi");
ls.SetKspType("cg");
Vec solution = ls.Solve(zero_guess);
unsigned cg_its = ls.GetNumIterations();
TS_ASSERT_EQUALS(cg_its, 40u);
TS_ASSERT_EQUALS(ls.mEigMin, DBL_MAX);
TS_ASSERT_EQUALS(ls.mEigMax, DBL_MIN);
PetscTools::Destroy(solution);
}
PetscTools::Destroy(system_matrix);
PetscTools::Destroy(system_rhs);
PetscTools::Destroy(parallel_layout);
PetscTools::Destroy(zero_guess);
}
bool ConjugateGradientSolver<Scalar>::solveWithoutPreconditioner(const LinearSystem<Scalar> &system,
const GeneralizedVector<Scalar> &b,
GeneralizedVector<Scalar> &x)
{
//see b2. of the reference for notations
this->iterations_used_ = 0;
Scalar tolerance_sqr = (this->tolerance_)*(this->tolerance_);
GeneralizedVector<Scalar> *r = b.clone(); //create a vector with the same type with b
system.multiply(x,*r); //r = Ax
(*r) *= -1;
(*r) += b; //r = b - Ax
system.filter(*r); //filter procedure to keep invariant of some components
GeneralizedVector<Scalar> *d = r->clone();
Scalar delta_0 = system.innerProduct(*r, *r);
Scalar delta = delta_0;
GeneralizedVector<Scalar> *q = d->clone();
GeneralizedVector<Scalar> *temp = d->clone();
this->residual_magnitude_sqr_ = delta;
while((this->iterations_used_ < this->max_iterations_) && (delta > tolerance_sqr*delta_0))
{
system.multiply(*d,*q);
system.filter(*q);
Scalar alpha = delta / (system.innerProduct(*d, *q));
*temp = *d;
*temp *= alpha;
x += *temp; //x = x + alpha*d
if((this->iterations_used_)%50 == 0) //direct compute residual every 50 iterations
{
system.multiply(x,*r);
(*r) *= -1;
(*r) += b; //r = b - Ax
system.filter(*r);
}
else
{
*temp = *q;
*temp *= alpha;
*r -= *temp; //r = r - alpha*q
}
Scalar delta_old = delta;
delta = system.innerProduct(*r, *r);
Scalar beta = delta/delta_old;
*temp = *d;
*temp *= beta;
*d = *r;
*d += *temp; //d = r + beta*d
system.filter(*d);
this->iterations_used_ = this->iterations_used_ + 1;
this->residual_magnitude_sqr_ = delta;
}
delete r;
delete d;
delete q;
delete temp;
//return false if didn't converge with maximum iterations
bool status = delta > tolerance_sqr*delta_0 ? false : true;
if (this->status_log_)
{
std::cout << "CG solver ";
if (!status)
std::cout << "did not converge";
else
std::cout << "converged";
std::cout << " in " << this->iterations_used_ << " iterations, residual: " << this->residualMagnitude() << ".\n";
}
return status;
}
void TestBasicFunctionality() throw (Exception)
{
/*
* We need to make sure here that the matrix is loaded with the appropriate parallel layout. Petsc's
* default puts 1331 rows in each processor. This wouldn't be possible in a real bidomain simulation
* because implies that equations V_665 an Phi_e_665 are solved in different processors.
*/
unsigned num_nodes = 1331;
DistributedVectorFactory factory(num_nodes);
Vec parallel_layout = factory.CreateVec(2);
Mat system_matrix;
PetscTools::ReadPetscObject(system_matrix, "linalg/test/data/matrices/cube_6000elems_half_activated.mat", parallel_layout);
PetscTools::Destroy(parallel_layout);
// Set rhs = A * [1 0 1 0 ... 1 0]'
Vec one_zeros = factory.CreateVec(2);
Vec rhs = factory.CreateVec(2);
for (unsigned node_index=0; node_index<2*num_nodes; node_index+=2)
{
PetscVecTools::SetElement(one_zeros, node_index, 1.0);
PetscVecTools::SetElement(one_zeros, node_index+1, 0.0);
}
PetscVecTools::Finalise(one_zeros);
MatMult(system_matrix, one_zeros, rhs);
PetscTools::Destroy(one_zeros);
LinearSystem ls = LinearSystem(rhs, system_matrix);
ls.SetAbsoluteTolerance(1e-9);
ls.SetKspType("cg");
ls.SetPcType("none");
ls.AssembleFinalLinearSystem();
Vec solution = ls.Solve();
DistributedVector distributed_solution = factory.CreateDistributedVector(solution);
DistributedVector::Stripe vm(distributed_solution, 0);
DistributedVector::Stripe phi_e(distributed_solution, 1);
for (DistributedVector::Iterator index = distributed_solution.Begin();
index!= distributed_solution.End();
++index)
{
/*
* Although we're trying to enforce the solution to be [1 0 ... 1 0], the system is singular and
* therefore it has infinite solutions. I've (migb) found that the use of different preconditioners
* lead to different solutions ([0.8 -0.2 ... 0.8 -0.2], [0.5 -0.5 ... 0.5 -0.5], ...)
*
* If we were using PETSc null space, it would find the solution that satisfies x'*v=0,
* being v the null space of the system (v=[1 1 ... 1])
*/
TS_ASSERT_DELTA(vm[index] - phi_e[index], 1.0, 1e-6);
}
// Coverage (setting PC type after first solve)
ls.SetPcType("blockdiagonal");
PetscTools::Destroy(system_matrix);
PetscTools::Destroy(rhs);
PetscTools::Destroy(solution);
#if (PETSC_VERSION_MAJOR == 3 && PETSC_VERSION_MINOR <= 3) //PETSc 3.0 to PETSc 3.3
//The PETSc developers changed this one, but later changed it back again!
const PCType pc;
#else
PCType pc;
#endif
PC prec;
KSPGetPC(ls.mKspSolver, &prec);
PCGetType(prec, &pc);
// Although we call it "blockdiagonal", PETSc considers this PC a generic SHELL preconditioner
TS_ASSERT( strcmp(pc,"shell")==0 );
}
void TestBetterThanNoPreconditioning()
{
unsigned num_nodes = 1331;
DistributedVectorFactory factory(num_nodes);
Vec parallel_layout = factory.CreateVec(2);
unsigned point_jacobi_its;
unsigned block_diag_its;
Timer::Reset();
{
Mat system_matrix;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_matrix, "linalg/test/data/matrices/cube_6000elems_half_activated.mat", parallel_layout);
Vec system_rhs;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_rhs, "linalg/test/data/matrices/cube_6000elems_half_activated.vec", parallel_layout);
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetAbsoluteTolerance(1e-9);
ls.SetKspType("cg");
ls.SetPcType("none");
Vec solution = ls.Solve();
point_jacobi_its = ls.GetNumIterations();
PetscTools::Destroy(system_matrix);
PetscTools::Destroy(system_rhs);
PetscTools::Destroy(solution);
}
Timer::PrintAndReset("No preconditioning");
{
Mat system_matrix;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_matrix, "linalg/test/data/matrices/cube_6000elems_half_activated.mat", parallel_layout);
Vec system_rhs;
// Note that this test deadlocks if the file's not on the disk
PetscTools::ReadPetscObject(system_rhs, "linalg/test/data/matrices/cube_6000elems_half_activated.vec", parallel_layout);
LinearSystem ls = LinearSystem(system_rhs, system_matrix);
ls.SetAbsoluteTolerance(1e-9);
ls.SetKspType("cg");
ls.SetPcType("blockdiagonal");
Vec solution = ls.Solve();
block_diag_its = ls.GetNumIterations();
// Coverage (setting PC type after using blockdiagonal solve)
ls.SetPcType("blockdiagonal");
PetscTools::Destroy(system_matrix);
PetscTools::Destroy(system_rhs);
PetscTools::Destroy(solution);
}
Timer::Print("Block diagonal preconditioner");
std::cout << block_diag_its << " " << point_jacobi_its << std::endl;
TS_ASSERT_LESS_THAN_EQUALS(block_diag_its, point_jacobi_its);
PetscTools::Destroy(parallel_layout);
}