-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathriccati.cpp
39 lines (37 loc) · 1.33 KB
/
riccati.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
#include <iostream>
#include "riccati.h"
void dare(const Eigen::MatrixXd &A,
const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q,
const Eigen::MatrixXd &R, Eigen::MatrixXd &P,
const double &tolerance,
const int iter_max,
const bool &verbose) {
P = Q;
Eigen::MatrixXd G = B * R.inverse() * B.transpose();
Eigen::MatrixXd Ak = A;
Eigen::MatrixXd Ak_next;
Eigen::MatrixXd P_next;
Eigen::MatrixXd G_next;
int m = A.rows();
int n = A.cols();
double diff;
if (verbose) {
std::cout << "Iter " << " " << " Error " << std::endl;
std::cout << "-----------------" << std::endl << std::endl;
}
for (int i=0; i < iter_max; ++i) {
Ak_next = Ak * (Eigen::MatrixXd::Identity(m, n) + G * P).inverse()*Ak;
G_next = G + Ak*(Eigen::MatrixXd::Identity(m, n) + G * P).inverse()*G*Ak.transpose();
P_next = P + Ak.transpose()*P*(Eigen::MatrixXd::Identity(m, n) + G * P).inverse()*Ak;
diff = fabs((P_next - P).maxCoeff());
P = P_next;
Ak = Ak_next;
G = G_next;
if (verbose) {
std::cout << i << " " << diff << std::endl;
}
if (diff < tolerance) {
return;
}
}
}