21 template <
size_t STATE_DIM,
size_t CONTROL_DIM,
typename SCALAR =
double>
25 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 const control_matrix_t& R_n,
47 const state_matrix_t& A_n,
48 const control_gain_matrix_t& B_n,
49 state_matrix_t& P_np1,
50 control_feedback_t& K_n)
52 control_matrix_t H_n = R_n;
53 H_n.noalias() += B_n.transpose() * P_np1 * B_n;
54 control_matrix_t H_n_inverse = H_n.inverse();
59 K_n.noalias() = -1.0 * H_n_inverse * B_n.transpose() * P_np1 * A_n;
62 P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
63 P_np1.noalias() -= K_n.transpose() * H_n * K_n;
65 P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
70 const control_matrix_t& R_n,
71 const state_matrix_t& A_n,
72 const control_gain_matrix_t& B_n,
73 state_matrix_t& P_np1,
74 control_feedback_t& K_n)
76 control_matrix_t H_n = R_n;
77 H_n.noalias() += B_n.transpose() * P_np1 * B_n;
78 H_n = (H_n + H_n.transpose()).eval() / 2.0;
81 eigenvalueSolver_.compute(H_n,
true);
82 const control_matrix_t& V = eigenvalueSolver_.eigenvectors().real();
83 const control_vector_t& lambda = eigenvalueSolver_.eigenvalues().real();
84 assert(eigenvalueSolver_.eigenvectors().imag().norm() < 1e-7 &&
"Eigenvectors not real");
85 assert(eigenvalueSolver_.eigenvalues().imag().norm() < 1e-7 &&
"Eigenvalues is not real");
87 control_matrix_t D = control_matrix_t::Zero();
89 D.diagonal() = lambda.cwiseMax(control_vector_t::Zero()) + epsilon_ * control_vector_t::Ones();
91 assert((V.transpose() - V.inverse()).norm() < 1e-5 &&
"WARNING: V transpose and V inverse are not similar!");
94 H_n.noalias() = V * D * V.transpose();
97 control_matrix_t D_inverse = control_matrix_t::Zero();
99 D_inverse.diagonal() = D.diagonal().cwiseInverse();
100 control_matrix_t H_n_inverse = V * D_inverse * V.transpose();
102 K_n.noalias() = -1.0 * H_n_inverse * (B_n.transpose() * P_np1 * A_n);
105 P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
106 P_np1.noalias() -= K_n.transpose() * H_n * K_n;
108 P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
114 Eigen::EigenSolver<control_matrix_t> eigenvalueSolver_;
void iterateNaive(const state_matrix_t &Q_n, const control_matrix_t &R_n, const state_matrix_t &A_n, const control_gain_matrix_t &B_n, state_matrix_t &P_np1, control_feedback_t &K_n)
Definition: DynamicRiccatiEquation.hpp:45
DynamicRiccatiEquation()
Definition: DynamicRiccatiEquation.hpp:35
Eigen::Matrix< SCALAR, CONTROL_DIM, 1 > control_vector_t
Definition: DynamicRiccatiEquation.hpp:29
Dynamic Riccati Equation.
Definition: DynamicRiccatiEquation.hpp:22
CppAD::AD< CppAD::cg::CG< double > > SCALAR
Eigen::Matrix< SCALAR, STATE_DIM, CONTROL_DIM > control_gain_matrix_t
Definition: DynamicRiccatiEquation.hpp:31
Eigen::Matrix< SCALAR, CONTROL_DIM, STATE_DIM > control_feedback_t
Definition: DynamicRiccatiEquation.hpp:32
~DynamicRiccatiEquation()
Definition: DynamicRiccatiEquation.hpp:37
Eigen::Matrix< SCALAR, CONTROL_DIM, STATE_DIM > control_state_matrix_t
Definition: DynamicRiccatiEquation.hpp:30
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Eigen::Matrix< SCALAR, STATE_DIM, STATE_DIM > state_matrix_t
Definition: DynamicRiccatiEquation.hpp:27
void iterateRobust(const state_matrix_t &Q_n, const control_matrix_t &R_n, const state_matrix_t &A_n, const control_gain_matrix_t &B_n, state_matrix_t &P_np1, control_feedback_t &K_n)
Definition: DynamicRiccatiEquation.hpp:69
Eigen::Matrix< SCALAR, CONTROL_DIM, CONTROL_DIM > control_matrix_t
Definition: DynamicRiccatiEquation.hpp:28