virtual std::ostream & printObject(std::ostream & os) { Eigen::IOFormat CleanFmt(Eigen::StreamPrecision, 0, ", ", "\n", "(", ")"); os << "Green's function type: spherical sharp" << std::endl; os << this->profile_ << std::endl; os << "Sphere center = " << this->origin_.transpose().format(CleanFmt); return os; }
std::ostream &operator<<(std::ostream &out, const HaWpParamSet<D> ¶meters) { Eigen::IOFormat CleanFmt(4, 0, ", ", "\n ", "[", "]"); out << "HaWpParamSet {\n"; out << " q: " << parameters.q().format(CleanFmt) << '\n'; out << " p: " << parameters.p().format(CleanFmt) << '\n'; out << " Q: " << parameters.Q().format(CleanFmt) << '\n'; out << " P: " << parameters.P().format(CleanFmt) << '\n'; out << " S: " << parameters.S() << '\n'; out << " sqrt(detQ): " << parameters.sdQ() << '\n'; out << " compatible(): " << (parameters.compatible() ? "yes" : "no") << '\n'; out << "}" << std::endl; return out; }
int main(int argc, const char** argv) { printf("start\n"); //string model_name = argv[1]; mj_activate("mjkey.txt"); m = mj_loadXML(argv[1], 0, 0, 0); d = mj_makeData(m); printf("Loaded model and made data\n"); d->qpos[0] = 1; d->qpos[1] = 1; EKF* ekf = new EKF(m, d, 1.0, 1.0, 1.0, 1e-3); mjData* d2 = mj_makeData(m); std::vector<mjData*> frame; MatrixXd deriv = ekf->get_deriv(m, d, d2); printf("Calc deriv done\n"); printf("nu: %d\n", m->nu); printf("L: %d\n", m->nq+m->nv); IOFormat CleanFmt(3, 0, ", ", "\n", "[", "]"); std::cout << deriv.rows() << "\t" << deriv.cols() << std::endl; std::cout << deriv.block(0, 0, m->nq+m->nv, m->nq+m->nv).format(CleanFmt) << std::endl; //MatrixXd sigma = MatrixXd::Identity(m->nq+m->nv+m->nv, m->nq+m->nv+m->nv); //printf("%f\n", d->qpos[10]); //double dt = m->opt.timestep; //ekf->predict_correct(d->ctrl, dt, d->sensordata, conf); }
std::ostream & operator<<(std::ostream &os, const Molecule &m) { // Declare formatting of Eigen output. std::string sep = " "; Eigen::IOFormat CleanFmt(Eigen::FullPrecision, Eigen::DontAlignCols, sep, "\n", "", ""); os << "Rotor type: " << rotorTypeList[m.rotor_] << std::endl; if (m.nAtoms_ != 0) { os << " Center X Y Z " << std::endl; os << " ------------ ----------------- ----------------- -----------------" << std::endl; for (size_t i = 0; i < m.nAtoms_; ++i) { os << std::setw(10) << m.atoms_[i].atomSymbol() << std::setw(15) <<m.geometry_.col( i).transpose().format(CleanFmt) << std::endl; } } else { os << " No atoms in this molecule!" << std::endl; } return os; }
ObserverBase::StateVector KalmanFilterBase::oneStepEstimation_() { unsigned k=this->x_.getTime(); BOOST_ASSERT(this->y_.size()> 0 && this->y_.checkIndex(k+1) && "ERROR: The measurement vector is not set"); BOOST_ASSERT(checkAmatrix(a_) && "ERROR: The Matrix A is not initialized" ); BOOST_ASSERT(checkCmatrix(c_) && "ERROR: The Matrix C is not initialized"); BOOST_ASSERT(checkQmatrix(q_) && "ERROR: The Matrix Q is not initialized"); BOOST_ASSERT(checkRmatrix(r_) && "ERROR: The Matrix R is not initialized"); BOOST_ASSERT(checkPmatrix(pr_) && "ERROR: The Matrix P is not initialized"); //prediction updatePredictedMeasurement();// runs also updatePrediction_(); oc_.pbar=q_; oc_.pbar.noalias() += a_*(pr_*a_.transpose()); //innovation Measurements oc_.inoMeas.noalias() = this->y_[k+1] - predictedMeasurement_; oc_.inoMeasCov.noalias() = r_ + c_ * (oc_.pbar * c_.transpose()); //inversing innovation measurement covariance matrix oc_.inoMeasCovLLT.compute(oc_.inoMeasCov); oc_.inoMeasCovInverse.resize(getMeasureSize(),getMeasureSize()); oc_.inoMeasCovInverse.setIdentity(); oc_.inoMeasCovLLT.matrixL().solveInPlace(oc_.inoMeasCovInverse); oc_.inoMeasCovLLT.matrixL().transpose().solveInPlace(oc_.inoMeasCovInverse); //innovation oc_.kGain.noalias() = oc_.pbar * (c_.transpose() * oc_.inoMeasCovInverse); inovation_.noalias() = oc_.kGain*oc_.inoMeas; //update oc_.xhat.noalias()= oc_.xbar+ inovation_; #ifdef VERBOUS_KALMANFILTER Eigen::IOFormat CleanFmt(2, 0, " ", "\n", "", ""); std::cout <<"A" <<std::endl<< a_.format(CleanFmt)<<std::endl; std::cout <<"C" <<std::endl<< c_.format(CleanFmt)<<std::endl; std::cout <<"P" <<std::endl<< pr_.format(CleanFmt)<<std::endl; std::cout <<"K" <<std::endl<< oc_.kGain.format(CleanFmt)<<std::endl; std::cout <<"Xbar" <<std::endl<< oc_.xbar.transpose().format(CleanFmt)<<std::endl; std::cout <<"inoMeasCov" <<std::endl<< oc_.inoMeasCov.format(CleanFmt)<<std::endl; std::cout <<"oc_.pbar" <<std::endl<< (oc_.pbar).format(CleanFmt)<<std::endl; std::cout <<"c_ * (oc_.pbar * c_.transpose())" <<std::endl<< ( c_ * (oc_.pbar * c_.transpose())).format(CleanFmt)<<std::endl; std::cout <<"inoMeasCovInverse" <<std::endl<< oc_.inoMeasCovInverse.format(CleanFmt)<<std::endl; std::cout <<"inoMeas" <<std::endl<< oc_.inoMeas.transpose().format(CleanFmt)<<std::endl; std::cout <<"inovation_" <<std::endl<< inovation_.transpose().format(CleanFmt)<<std::endl; std::cout <<"Xhat" <<std::endl<< oc_.xhat.transpose().format(CleanFmt)<<std::endl; #endif // VERBOUS_KALMANFILTER this->x_.set(oc_.xhat,k+1); pr_=oc_.stateIdentity; pr_ -= oc_.kGain*c_; pr_ *= oc_.pbar; // simmetrize the pr_ matrix oc_.t.noalias()=pr_; oc_.t+=pr_.transpose(); pr_=0.5*oc_.t; return oc_.xhat; }