State Space Models (SSM)

1
#include <bits/stdc++.h>
2
using namespace std;
3
4
// Basic linear algebra helpers for small dense matrices
5
using Mat = vector<vector<double>>;
6
using Vec = vector<double>;
7
8
Vec matVec(const Mat& A, const Vec& x){
9
    size_t n = A.size(); size_t m = x.size();
10
    Vec y(n, 0.0);
11
    for(size_t i=0;i<n;++i){
12
        for(size_t j=0;j<m;++j) y[i] += A[i][j]*x[j];
13
    }
14
    return y;
15
}
16
17
Mat matMul(const Mat& A, const Mat& B){
18
    size_t n=A.size(), k=A[0].size(), m=B[0].size();
19
    Mat C(n, vector<double>(m,0.0));
20
    for(size_t i=0;i<n;++i)
21
        for(size_t t=0;t<k;++t){ double a=A[i][t];
22
            for(size_t j=0;j<m;++j) C[i][j]+=a*B[t][j]; }
23
    return C;
24
}
25
26
Vec vecAdd(const Vec& a, const Vec& b){ Vec c=a; for(size_t i=0;i<a.size();++i) c[i]+=b[i]; return c; }
27
Vec vecScale(const Vec& a, double s){ Vec c=a; for(double &v:c) v*=s; return c; }
28
29
// Derivative: dx/dt = A x + B u(t)
30
Vec f(const Mat& A, const Mat& B, const Vec& x, const Vec& u){
31
    Vec Ax = matVec(A, x);
32
    // Add Bu
33
    Vec Bu(A.size(), 0.0);
34
    for(size_t i=0;i<A.size();++i)
35
        for(size_t j=0;j<B[0].size();++j)
36
            Bu[i] += B[i][j]*u[j];
37
    for(size_t i=0;i<A.size();++i) Ax[i]+=Bu[i];
38
    return Ax;
39
}
40
41
int main(){
42
    // Example: mass-spring-damper m xdd + c xd + k x = u
43
    // State x = [position, velocity]^T
44
    double m=1.0, c=0.5, k=4.0;
45
    Mat A = {{0.0, 1.0}, {-k/m, -c/m}}; // 2x2
46
    Mat B = {{0.0}, {1.0/m}};          // 2x1
47
    Mat C = {{1.0, 0.0}};              // output is position
48
    Mat D = {{0.0}};
49
50
    // Input: step force u(t)=1 for t>=0
51
    auto u_of_t = [&](double t)->Vec{
52
        (void)t; return Vec{1.0};
53
    };
54
55
    // RK4 parameters
56
    double dt = 0.001; // time step (s)
57
    int steps = 10000; // simulate 10 s
58
    Vec x = {0.0, 0.0}; // initial position, velocity
59
60
    cout << fixed << setprecision(6);
61
    cout << "# t position velocity output\n";
62
    double t=0.0;
63
    for(int kstep=0;kstep<steps;++kstep){
64
        Vec u = u_of_t(t);
65
        // RK4 stages
66
        Vec k1 = f(A,B,x,u);
67
        Vec k2 = f(A,B, vecAdd(x, vecScale(k1, dt*0.5)), u);
68
        Vec k3 = f(A,B, vecAdd(x, vecScale(k2, dt*0.5)), u);
69
        Vec k4 = f(A,B, vecAdd(x, vecScale(k3, dt)), u);
70
        Vec dx = vecScale(vecAdd(vecAdd(k1, vecScale(vecAdd(k2,k3),2.0)), k4), dt/6.0);
71
        // Update state
72
        x = vecAdd(x, dx);
73
        // Output y = Cx + Du
74
        Vec y = matVec(C, x);
75
        for(size_t i=0;i<D.size();++i)
76
            for(size_t j=0;j<D[0].size();++j)
77
                y[i] += D[i][j]*u[j];
78
        if(kstep % 100 == 0)
79
            cout << t << " " << x[0] << " " << x[1] << " " << y[0] << "\n";
80
        t += dt;
81
    }
82
    return 0;
83
}
84

1
#include <bits/stdc++.h>
2
using namespace std;
3
using Mat = vector<vector<double>>;
4
5
Mat eye(size_t n){ Mat I(n, vector<double>(n,0.0)); for(size_t i=0;i<n;++i) I[i][i]=1.0; return I; }
6
Mat matMul(const Mat& A, const Mat& B){ size_t n=A.size(), k=A[0].size(), m=B[0].size(); Mat C(n, vector<double>(m,0.0)); for(size_t i=0;i<n;++i) for(size_t t=0;t<k;++t){ double a=A[i][t]; for(size_t j=0;j<m;++j) C[i][j]+=a*B[t][j]; } return C; }
7
Mat matAdd(const Mat& A, const Mat& B){ Mat C=A; for(size_t i=0;i<A.size();++i) for(size_t j=0;j<A[0].size();++j) C[i][j]+=B[i][j]; return C; }
8
Mat matScale(const Mat& A, double s){ Mat C=A; for(auto &r:C) for(double &v:r) v*=s; return C; }
9
10
// Compute (A*dt)^k iteratively up to K and use truncation to approximate exp and integral
11
pair<Mat, Mat> discretizeZOH_series(const Mat& A, const Mat& B, double dt, int K){
12
    size_t n=A.size(); size_t m=B[0].size();
13
    Mat Ad = eye(n); // start with I for exp series
14
    Mat term = eye(n); // (A dt)^0
15
    // Series for A_d = sum_{k=0}^K (A dt)^k / k!
16
    double fact = 1.0;
17
    Mat A_dt = matScale(A, dt);
18
    for(int k=1;k<=K;++k){
19
        term = matMul(term, A_dt); // (A dt)^k
20
        fact *= k; // k!
21
        Ad = matAdd(Ad, matScale(term, 1.0/fact));
22
    }
23
    // Series for B_d = sum_{k=0}^K (A dt)^k * dt/(k+1)! * B
24
    Mat Bd(n, vector<double>(m, 0.0));
25
    term = eye(n); fact = 1.0; // reset to (A dt)^0 / 0!
26
    for(int k=0;k<=K;++k){
27
        double coeff = dt / (fact * (k+1)); // dt/(k+1)!
28
        Mat contrib = matScale(term, coeff);
29
        Mat add = matMul(contrib, B);
30
        // Bd += add
31
        for(size_t i=0;i<n;++i) for(size_t j=0;j<m;++j) Bd[i][j] += add[i][j];
32
        // next power
33
        term = matMul(term, A_dt);
34
        fact *= (k+1 == 0 ? 1 : (k+1)); // maintain (k+1)! in denominator for next loop
35
    }
36
    return {Ad, Bd};
37
}
38
39
int main(){
40
    // Same mass-spring-damper as before
41
    double m=1.0, c=0.5, k=4.0; (void)m;
42
    vector<vector<double>> A = {{0.0, 1.0}, {-k/m, -c/m}};
43
    vector<vector<double>> B = {{0.0}, {1.0/m}};
44
45
    double dt = 0.01; // sample time
46
    int K = 6;        // truncation order (increase for accuracy)
47
    auto [Ad, Bd] = discretizeZOH_series(A, B, dt, K);
48
49
    cout << fixed << setprecision(6);
50
    cout << "Ad =\n";
51
    for(auto &r:Ad){ for(double v:r) cout << setw(12) << v << ' '; cout << '\n'; }
52
    cout << "Bd =\n";
53
    for(auto &r:Bd){ for(double v:r) cout << setw(12) << v << ' '; cout << '\n'; }
54
55
    // Test one-step response to unit input from zero state: x1 = Ad x0 + Bd u0
56
    vector<double> x0 = {0.0, 0.0};
57
    vector<double> u0 = {1.0};
58
    // x1 = Bd * u0 since x0=0
59
    vector<double> x1(2,0.0);
60
    for(size_t i=0;i<2;++i) for(size_t j=0;j<1;++j) x1[i] += Bd[i][j]*u0[j];
61
    cout << "x1 from rest with u=1: [" << x1[0] << ", " << x1[1] << "]\n";
62
    return 0;
63
}
64

1
#include <bits/stdc++.h>
2
using namespace std; using Mat = vector<vector<double>>;
3
4
Mat matMul(const Mat& A, const Mat& B){ size_t n=A.size(), k=A[0].size(), m=B[0].size(); Mat C(n, vector<double>(m,0.0)); for(size_t i=0;i<n;++i) for(size_t t=0;t<k;++t){ double a=A[i][t]; for(size_t j=0;j<m;++j) C[i][j]+=a*B[t][j]; } return C; }
5
Mat horzCat(const Mat& A, const Mat& B){ Mat C=A; for(size_t i=0;i<A.size();++i) C[i].insert(C[i].end(), B[i].begin(), B[i].end()); return C; }
6
Mat vertCat(const Mat& A, const Mat& B){ Mat C=A; for(auto r:B) C.push_back(r); return C; }
7
8
int rankGauss(Mat M, double eps=1e-9){
9
    int n=M.size(), m=M[0].size(), r=0;
10
    for(int c=0;c<m && r<n; ++c){
11
        int piv=r; for(int i=r;i<n;++i) if(fabs(M[i][c])>fabs(M[piv][c])) piv=i;
12
        if(fabs(M[piv][c])<eps) continue; swap(M[piv], M[r]);
13
        double div=M[r][c]; for(int j=c;j<m;++j) M[r][j]/=div;
14
        for(int i=0;i<n;++i) if(i!=r){ double f=M[i][c]; if(fabs(f)<eps) continue; for(int j=c;j<m;++j) M[i][j]-=f*M[r][j]; }
15
        ++r;
16
    }
17
    return r;
18
}
19
20
Mat controllabilityMatrix(const Mat& A, const Mat& B){
21
    size_t n=A.size(); Mat Cn=B; Mat Ab=B; // Ab = A^k B (k starts at 1)
22
    Mat Ak = A; // A^1
23
    for(size_t k=1;k<n; ++k){
24
        Ab = matMul(Ak, B);
25
        Cn = horzCat(Cn, Ab);
26
        Ak = matMul(Ak, A); // next power
27
    }
28
    return Cn;
29
}
30
31
Mat observabilityMatrix(const Mat& A, const Mat& C){
32
    size_t n=A.size(); Mat On=C; Mat Ak = A; // A^1
33
    for(size_t k=1;k<n; ++k){ Mat row = matMul(C, Ak); On = vertCat(On, row); Ak = matMul(Ak, A); }
34
    return On;
35
}
36
37
int main(){
38
    // Example 1: controllable and observable mass-spring-damper
39
    double m=1.0, c=0.5, k=4.0;
40
    Mat A1 = {{0.0, 1.0}, {-k/m, -c/m}}; Mat B1 = {{0.0}, {1.0/m}}; Mat C1 = {{1.0, 0.0}};
41
    Mat Cmat1 = controllabilityMatrix(A1, B1); Mat Omat1 = observabilityMatrix(A1, C1);
42
    cout << "Rank(C1)=" << rankGauss(Cmat1) << ", Rank(O1)=" << rankGauss(Omat1) << " (n=2)\n";
43
44
    // Example 2: uncontrollable system: dx1= x2, dx2=0, input has no effect (B=0)
45
    Mat A2 = {{0.0, 1.0}, {0.0, 0.0}}; Mat B2 = {{0.0}, {0.0}}; Mat C2 = {{1.0, 0.0}};
46
    Mat Cmat2 = controllabilityMatrix(A2, B2); Mat Omat2 = observabilityMatrix(A2, C2);
47
    cout << "Rank(C2)=" << rankGauss(Cmat2) << ", Rank(O2)=" << rankGauss(Omat2) << " (n=2)\n";
48
    return 0;
49
}
50