9 void pinv(
const Eigen::MatrixXd& matrix_in, Eigen::MatrixXd& pseudo_inv,
const double& pinvtoler) {
10 Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix_in, Eigen::ComputeThinU | Eigen::ComputeThinV);
11 Eigen::VectorXd singular_values;
12 Eigen::VectorXd singular_values_inv;
13 singular_values = svd.singularValues();
14 singular_values_inv.setZero(singular_values.size());
16 for (
int w = 0; w < singular_values.size(); ++w)
17 if (singular_values(w) > pinvtoler) singular_values_inv(w) = 1 / singular_values(w);
18 pseudo_inv = svd.matrixV() * singular_values_inv.asDiagonal() * svd.matrixU().transpose();
23 : order_(order), N_(N), dt_(dt), dt_zero_(true), first_run_(true), pt_(0) {
45 for (
unsigned int i = 0; i < esteem.size(); ++i) esteem.at(i) = el[i];
58 for (
unsigned int j = 0; j <
N_; ++j) {
60 if (idx >=
N_) idx = idx -
N_;
69 for (
unsigned int i = 0; i < dim; ++i) {
71 for (
unsigned int j = 0; j <
N_; ++j) {
73 if (idx >=
N_) idx = idx -
N_;
85 for (
unsigned int i = 0; i <
N_; ++i) {
89 for (
unsigned int j = 1; j <=
order_; ++j) {
94 Eigen::Map<Eigen::VectorXd> ytemp(&
x_[0],
N_, 1);
95 coeff_ =
R_.householderQr().solve(ytemp);