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

// ssdata.cpp
// 编译: g++ -std=c++11 -O2 ssdata.cpp -o ssdata
// 运行: ./ssdata (Linux/macOS) 或 ssdata.exe (Windows)

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

// ============================================================
// 轻量矩阵类 (仅实现本示例所需功能, 不依赖任何第三方库)
// ============================================================
class Matrix {
public:
int rows;
int cols;
std::vector<double> data; // 行优先存储

Matrix() : rows(0), cols(0) {}

Matrix(int r, int c, double init = 0.0)
: rows(r), cols(c), data(r * c, init) {}

double& operator()(int i, int j) { return data[i * cols + j]; }
double operator()(int i, int j) const { return data[i * cols + j]; }

static Matrix Zero(int r, int c) { return Matrix(r, c, 0.0); }

static Matrix Identity(int n) {
Matrix I(n, n, 0.0);
for (int i = 0; i < n; ++i) I(i, i) = 1.0;
return I;
}

void print(const std::string& name) const {
std::cout << name << " =\\n";
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
double v = (*this)(i, j);
if (std::abs(v) < 1e-12) v = 0.0; // 消除 -0 显示
std::cout << std::setw(10) << std::setprecision(6)
<< std::fixed << v;
if (j + 1 < cols) std::cout << " ";
}
std::cout << "\\n";
}
}
};

// ============================================================
// 状态空间模型类 (对应 MATLAB ssdata 的功能)
// ============================================================
class StateSpaceModel {
public:
Matrix A, B, C, D;
double Ts; // 采样时间, 0 表示连续时间

StateSpaceModel() : Ts(0.0) {}

StateSpaceModel(const Matrix& A_, const Matrix& B_,
const Matrix& C_, const Matrix& D_,
double Ts_ = 0.0)
: A(A_), B(B_), C(C_), D(D_), Ts(Ts_) {
validateDimensions();
}

// 从传递函数构造 (可控标准型), 仅支持 SISO
// num / den: 系数从最高次到最低次
static StateSpaceModel fromTransferFunction(const std::vector<double>& num,
const std::vector<double>& den,
double Ts = 0.0) {
if (den.empty() || std::abs(den[0]) < 1e-12)
throw std::invalid_argument("Denominator leading coefficient must be non-zero.");

int n = static_cast<int>(den.size()) – 1; // 系统阶数
if (n <= 0)
throw std::invalid_argument("Transfer function must have order >= 1.");

// 规范化分母
std::vector<double> a(den.size());
for (size_t i = 0; i < den.size(); ++i) a[i] = den[i] / den[0];

// 分子补齐到 n+1 位
std::vector<double> b(n + 1, 0.0);
int offset = (n + 1) – static_cast<int>(num.size());
for (size_t i = 0; i < num.size(); ++i)
b[offset + i] = num[i] / den[0];

Matrix A = Matrix::Zero(n, n);
Matrix B = Matrix::Zero(n, 1);
Matrix C = Matrix::Zero(1, n);
Matrix D = Matrix::Zero(1, 1);

// 友矩阵形式 (可控标准型)
for (int i = 0; i < n; ++i)
A(n – 1, i) = –a[n – i]; // 最后一行
for (int i = 0; i < n – 1; ++i)
A(i, i + 1) = 1.0;
B(n – 1, 0) = 1.0;

// 输出矩阵
for (int i = 0; i < n; ++i)
C(0, i) = b[n – i] – a[n – i] * b[0];
D(0, 0) = b[0];

return StateSpaceModel(A, B, C, D, Ts);
}

// 提取状态空间数据 —— 对应 MATLAB 的 ssdata
void ssdata(Matrix& A_out, Matrix& B_out,
Matrix& C_out, Matrix& D_out) const {
A_out = A; B_out = B;
C_out = C; D_out = D;
}

void ssdata(Matrix& A_out, Matrix& B_out,
Matrix& C_out, Matrix& D_out,
double& Ts_out) const {
ssdata(A_out, B_out, C_out, D_out);
Ts_out = Ts;
}

void validateDimensions() const {
if (A.rows != A.cols)
throw std::invalid_argument("A must be square.");
int n = A.rows;
if (B.rows != n)
throw std::invalid_argument("B must have n rows.");
if (C.cols != n)
throw std::invalid_argument("C must have n columns.");
if (D.rows != C.rows || D.cols != B.cols)
throw std::invalid_argument("D must be p x m.");
}
};

// ============================================================
// 主程序
// ============================================================
int main() {
try {
// 示例: G(s) = (2s + 3) / (s^2 + 2s + 5)
std::vector<double> num = {2, 3};
std::vector<double> den = {1, 2, 5};

StateSpaceModel sys = StateSpaceModel::fromTransferFunction(num, den);

Matrix A, B, C, D;
double Ts = 0.0;
sys.ssdata(A, B, C, D, Ts); // 对应 MATLAB: [A,B,C,D,Ts] = ssdata(sys)

A.print("A");
std::cout << "\\n";
B.print("B");
std::cout << "\\n";
C.print("C");
std::cout << "\\n";
D.print("D");
std::cout << "\\n";

std::cout << std::defaultfloat; // 恢复默认浮点显示
std::cout << "Ts = " << Ts << std::endl;
}
catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
return 0;
}