C++代码实现MATLAB中的care函数功能

// care.cpp — 自包含 CARE 求解器 (无需任何第三方库)

// 编译: g++ -std=c++17 -O2 care.cpp -o care
// 运行: ./care

#include <iostream>
#include <vector>
#include <cmath>
#include <iomanip>
#include <stdexcept>
#include <string>
#include <algorithm>

using Matrix = std::vector<std::vector<double>>;
using Vector = std::vector<double>;

// ———————————————————————
// 基础矩阵运算
// ———————————————————————

Matrix zeros(int r, int c) { return Matrix(r, Vector(c, 0.0)); }

Matrix eye(int n) {
Matrix I = zeros(n, n);
for (int i = 0; i < n; ++i) I[i][i] = 1.0;
return I;
}

Matrix transpose(const Matrix& A) {
int r = (int)A.size(), c = (int)A[0].size();
Matrix T = zeros(c, r);
for (int i = 0; i < r; ++i)
for (int j = 0; j < c; ++j)
T[j][i] = A[i][j];
return T;
}

Matrix operator*(const Matrix& A, const Matrix& B) {
int n = (int)A.size(), m = (int)A[0].size(), p = (int)B[0].size();
Matrix C = zeros(n, p);
for (int i = 0; i < n; ++i)
for (int k = 0; k < m; ++k) {
double a = A[i][k];
if (a == 0.0) continue;
for (int j = 0; j < p; ++j)
C[i][j] += a * B[k][j];
}
return C;
}

Matrix operator+(const Matrix& A, const Matrix& B) {
int r = (int)A.size(), c = (int)A[0].size();
Matrix C = zeros(r, c);
for (int i = 0; i < r; ++i)
for (int j = 0; j < c; ++j)
C[i][j] = A[i][j] + B[i][j];
return C;
}

Matrix operator–(const Matrix& A, const Matrix& B) {
int r = (int)A.size(), c = (int)A[0].size();
Matrix C = zeros(r, c);
for (int i = 0; i < r; ++i)
for (int j = 0; j < c; ++j)
C[i][j] = A[i][j] – B[i][j];
return C;
}

Matrix operator*(double s, const Matrix& A) {
int r = (int)A.size(), c = (int)A[0].size();
Matrix C = zeros(r, c);
for (int i = 0; i < r; ++i)
for (int j = 0; j < c; ++j)
C[i][j] = s * A[i][j];
return C;
}

// ———————————————————————
// 高斯消元求逆（带部分主元选取）
// ———————————————————————

Matrix inv(const Matrix& A) {
int n = (int)A.size();
Matrix aug = zeros(n, 2 * n);
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) aug[i][j] = A[i][j];
aug[i][n + i] = 1.0;
}
for (int col = 0; col < n; ++col) {
int piv = col;
double best = std::abs(aug[col][col]);
for (int r = col + 1; r < n; ++r) {
if (std::abs(aug[r][col]) > best) { best = std::abs(aug[r][col]); piv = r; }
}
if (best < 1e-12) throw std::runtime_error("Singular matrix");
std::swap(aug[col], aug[piv]);
double d = aug[col][col];
for (int j = 0; j < 2 * n; ++j) aug[col][j] /= d;
for (int r = 0; r < n; ++r) {
if (r == col) continue;
double f = aug[r][col];
if (f == 0.0) continue;
for (int j = 0; j < 2 * n; ++j) aug[r][j] -= f * aug[col][j];
}
}
Matrix R = zeros(n, n);
for (int i = 0; i < n; ++i)
for (int j = 0; j < n; ++j)
R[i][j] = aug[i][n + j];
return R;
}

// ———————————————————————
// Kronecker 积 A ⊗ B
// ———————————————————————

Matrix kron(const Matrix& A, const Matrix& B) {
int ra = (int)A.size(), ca = (int)A[0].size();
int rb = (int)B.size(), cb = (int)B[0].size();
Matrix C = zeros(ra * rb, ca * cb);
for (int i = 0; i < ra; ++i)
for (int j = 0; j < ca; ++j)
for (int k = 0; k < rb; ++k)
for (int l = 0; l < cb; ++l)
C[i * rb + k][j * cb + l] = A[i][j] * B[k][l];
return C;
}

// ———————————————————————
// 求解 Lyapunov 方程: A·X + X·B = C
// 利用 vec 展开: (I ⊗ A + B^T ⊗ I) vec(X) = vec(C)
// ———————————————————————

Matrix solve_lyap(const Matrix& A, const Matrix& B, const Matrix& C) {
int n = (int)A.size();
Matrix I = eye(n);
Matrix M = kron(I, A) + kron(transpose(B), I);
Vector vc(n * n);
for (int j = 0; j < n; ++j)
for (int i = 0; i < n; ++i)
vc[j * n + i] = C[i][j]; // 列优先展开
Matrix Minv = inv(M);
Vector vx(n * n, 0.0);
for (int i = 0; i < n * n; ++i) {
double s = 0.0;
for (int j = 0; j < n * n; ++j) s += Minv[i][j] * vc[j];
vx[i] = s;
}
Matrix X = zeros(n, n);
for (int j = 0; j < n; ++j)
for (int i = 0; i < n; ++i)
X[i][j] = vx[j * n + i];
return X;
}

// ———————————————————————
// 正定性检查（Cholesky 分解）
// ———————————————————————

bool is_pos_def(const Matrix& A) {
int n = (int)A.size();
Matrix L = zeros(n, n);
for (int i = 0; i < n; ++i) {
for (int j = 0; j <= i; ++j) {
double s = A[i][j];
for (int k = 0; k < j; ++k) s -= L[i][k] * L[j][k];
if (i == j) {
if (s <= 1e-10) return false;
L[i][j] = std::sqrt(s);
} else {
L[i][j] = s / L[j][j];
}
}
}
return true;
}

// ———————————————————————
// Ackermann 极点配置: 找初始稳定增益 K
// 目标极点: -1, -2, …, -n
// K = [0 … 0 1]·C⁻¹·φ(A), φ(s) = (s+1)ⁿ, C = [B, AB, …, Aⁿ⁻¹B]
// ———————————————————————

Matrix find_initial_K(const Matrix& A, const Matrix& B) {
int n = (int)A.size();
int m = (int)B[0].size();
for (int col = 0; col < m; ++col) {
Matrix b = zeros(n, 1);
for (int i = 0; i < n; ++i) b[i][0] = B[i][col];
// 能控性矩阵 C = [b, Ab, A²b, …]
Matrix C = zeros(n, n);
Matrix Akb = b;
for (int k = 0; k < n; ++k) {
for (int i = 0; i < n; ++i) C[i][k] = Akb[i][0];
Akb = A * Akb;
}
try {
Matrix Cinv = inv(C);
Matrix AI = A + eye(n);
Matrix phi = eye(n);
for (int k = 0; k < n; ++k) phi = phi * AI; // φ(A) = (A+I)ⁿ
Matrix tmp = Cinv * phi;
Matrix K = zeros(m, n);
for (int j = 0; j < n; ++j) K[col][j] = tmp[n – 1][j];
return K;
} catch (...) {
continue; // 该列不可控，尝试下一列
}
}
throw std::runtime_error("Cannot find stabilizing gain (uncontrollable?)");
}

// ———————————————————————
// Kleinman 迭代求解 CARE:
// AᵀX + XA – XB R⁻¹ BᵀX + Q = 0
// ———————————————————————

Matrix care(const Matrix& A, const Matrix& B,
const Matrix& Q, const Matrix& R,
Matrix& G, int max_iter = 100, double tol = 1e-12)
{
int n = (int)A.size();
int m = (int)B[0].size();
Matrix Rinv = inv(R);
Matrix Bt = transpose(B);
Matrix K = find_initial_K(A, B); // 保证 A – BK 稳定
Matrix X = zeros(n, n);
for (int iter = 0; iter < max_iter; ++iter) {
Matrix Ak = A – B * K;
// 求解 Lyapunov: Akᵀ X + X Ak = -(Q + Kᵀ R K)
Matrix RHS = –1.0 * (Q + transpose(K) * R * K);
X = solve_lyap(transpose(Ak), Ak, RHS);
// 强制对称
for (int i = 0; i < n; ++i)
for (int j = 0; j < i; ++j) {
double avg = 0.5 * (X[i][j] + X[j][i]);
X[i][j] = X[j][i] = avg;
}
Matrix Knew = Rinv * Bt * X;
double diff = 0.0;
for (int i = 0; i < m; ++i)
for (int j = 0; j < n; ++j)
diff += std::abs(Knew[i][j] – K[i][j]);
K = Knew;
if (diff < tol) break;
}
G = K;
return X;
}

// ———————————————————————
// 2×2 矩阵特征值（解析）
// ———————————————————————

void eig2x2(const Matrix& M,
double& re1, double& im1, double& re2, double& im2) {
double a = M[0][0], b = M[0][1], c = M[1][0], d = M[1][1];
double tr = a + d;
double det = a * d – b * c;
double disc = tr * tr – 4.0 * det;
if (disc >= 0) {
double s = std::sqrt(disc);
re1 = (tr + s) / 2; im1 = 0.0;
re2 = (tr – s) / 2; im2 = 0.0;
} else {
double s = std::sqrt(–disc);
re1 = re2 = tr / 2.0;
im1 = s / 2.0;
im2 = –s / 2.0;
}
}

// ———————————————————————
// 打印辅助
// ———————————————————————

void print_matrix(const Matrix& M, const std::string& name) {
std::cout << name << " =\\n";
for (const auto& row : M) {
std::cout << " ";
for (double v : row) {
if (std::abs(v) < 1e-13) v = 0.0;
std::cout << std::setw(12) << v << " ";
}
std::cout << "\\n";
}
std::cout << "\\n";
}

void print_complex(double re, double im) {
std::cout << std::fixed << std::setprecision(6) << re;
if (im >= 0) std::cout << " + " << im << "i";
else std::cout << " – " << –im << "i";
}

// ———————————————————————
// 主函数
// ———————————————————————

int main() {
std::cout << std::fixed << std::setprecision(6);
// 双积分器系统: x'' = u
Matrix A = {{0.0, 1.0},
{0.0, 0.0}};
Matrix B = {{0.0},
{1.0}};
Matrix Q = eye(2);
Matrix R = {{1.0}};
std::cout << "求解 CARE: A^T X + X A – X B R^{-1} B^T X + Q = 0\\n\\n";
print_matrix(A, "A");
print_matrix(B, "B");
print_matrix(Q, "Q");
print_matrix(R, "R");
Matrix G;
Matrix X = care(A, B, Q, R, G);
print_matrix(X, "X");
print_matrix(G, "G (反馈增益)");
// 闭环极点
Matrix Acl = A – B * G;
double re1, im1, re2, im2;
eig2x2(Acl, re1, im1, re2, im2);
std::cout << "闭环极点:\\n ";
print_complex(re1, im1); std::cout << "\\n ";
print_complex(re2, im2); std::cout << "\\n";
return 0;
}