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;
}
