- 3.0.2 optimal control module.
DynamicRiccatiEquation.hpp
Go to the documentation of this file.
1 /**********************************************************************************************************************
2 This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
3 Licensed under the BSD-2 license (see LICENSE file in main directory)
4 **********************************************************************************************************************/
5 
6 #pragma once
7 
8 namespace ct {
9 namespace optcon {
10 
21 template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
23 {
24 public:
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 
27  typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
28  typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
29  typedef Eigen::Matrix<SCALAR, CONTROL_DIM, 1> control_vector_t;
30  typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
31  typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
32  typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
33 
34 
35  DynamicRiccatiEquation() : epsilon_(1e-5), eigenvalueSolver_(CONTROL_DIM){};
36 
38 
39  // Naive, direct literal implementation
40  // computes P = Q + A^T ( P - P B ( R + B^T P B )^-1 B^T P) A.
41  // = Q + A^T ( P - P B H_inverse B^T P) A
42  // = Q + A^T S A
43  // where H_inverse = (R + B^T P B)^-1 and S = P - P B H_inverse B^T * P
44  // i.e. P is altered and H_inverse is returned for convenience and use in other functions
45  void iterateNaive(const state_matrix_t& Q_n,
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)
51  {
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();
55 
56  //std::cout << "H_n (naive)" << std::endl << H_n << std::endl;
57  //std::cout << "H_n_inverse (naive)" << std::endl << H_n.inverse() << std::endl;
58 
59  K_n.noalias() = -1.0 * H_n_inverse * B_n.transpose() * P_np1 * A_n;
60 
61  // here we compute P_n
62  P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
63  P_np1.noalias() -= K_n.transpose() * H_n * K_n;
64 
65  P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
66  }
67 
68 
69  void iterateRobust(const state_matrix_t& Q_n,
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)
75  {
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;
79 
80  // compute eigenvalues with eigenvectors enabled
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");
86  // Corrected Eigenvalue Matrix
87  control_matrix_t D = control_matrix_t::Zero();
88  // make D positive semi-definite (as described in IV. B.)
89  D.diagonal() = lambda.cwiseMax(control_vector_t::Zero()) + epsilon_ * control_vector_t::Ones();
90 
91  assert((V.transpose() - V.inverse()).norm() < 1e-5 && "WARNING: V transpose and V inverse are not similar!");
92 
93  // reconstruct H
94  H_n.noalias() = V * D * V.transpose();
95 
96  // invert D
97  control_matrix_t D_inverse = control_matrix_t::Zero();
98  // eigenvalue-wise inversion
99  D_inverse.diagonal() = D.diagonal().cwiseInverse();
100  control_matrix_t H_n_inverse = V * D_inverse * V.transpose();
101 
102  K_n.noalias() = -1.0 * H_n_inverse * (B_n.transpose() * P_np1 * A_n);
103 
104  // here we compute P_n
105  P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
106  P_np1.noalias() -= K_n.transpose() * H_n * K_n;
107 
108  P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
109  }
110 
111 
112 private:
113  SCALAR epsilon_;
114  Eigen::EigenSolver<control_matrix_t> eigenvalueSolver_;
115 };
116 
117 
118 } // namespace optcon
119 } // namespace ct
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