oxictl 0.1.1

Pure Rust Real-Time Control Systems Framework
Documentation 
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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203

//! H2 optimal control via DARE.
//!
//! The H2 optimal controller minimizes the H2 norm of the closed-loop
//! transfer function, equivalent to minimizing the LQR cost with specific
//! disturbance input structure.
#![allow(unused)]

use crate::core::matrix::{matmul, Matrix};
use crate::core::scalar::ControlScalar;
use crate::state_feedback::lqr::solve_dare;

/// Solve H2 optimal control problem using DARE.
///
/// Returns optimal gain K (I×N) such that u = -K*x minimizes the H2 cost.
/// A: N×N state matrix, B: N×I input matrix, Q: N×N state cost, R: I×I input cost.
/// Returns None if DARE does not converge or R+B^T P B is singular.
pub fn solve_h2_dare<S: ControlScalar, const N: usize, const I: usize>(
    a: &Matrix<S, N, N>,
    b: &Matrix<S, N, I>,
    q: &Matrix<S, N, N>,
    r: &Matrix<S, I, I>,
) -> Option<(Matrix<S, I, N>, Matrix<S, N, N>)> {
    let sol = solve_dare(a, b, q, r, 2000, S::from_f64(1e-10))?;
    Some((sol.k, sol.p))
}

/// H2 norm bound: trace(B_d^T P B_d) where P is the cost matrix.
///
/// Gives an upper bound on the H2 norm of the closed-loop system
/// from disturbance input B_d to the regulated output.
pub fn h2_norm_bound<S: ControlScalar, const N: usize, const I: usize>(
    p: &Matrix<S, N, N>,
    b_disturbance: &Matrix<S, N, I>,
) -> S {
    // B_d^T P B_d (I×I)
    let bt = b_disturbance.transpose();
    let pb = matmul(p, b_disturbance);
    let btpb = matmul(&bt, &pb);
    btpb.trace()
}

/// H2 Controller: u = -K*x with H2-optimal gain.
///
/// Derived from DARE solution with the given state and input weights.
/// The controller minimises H2 performance criterion.
#[derive(Debug, Clone, Copy)]
pub struct H2Controller<S: ControlScalar, const N: usize, const I: usize> {
    /// Optimal gain matrix K (I×N).
    pub gain: Matrix<S, I, N>,
    /// H2 norm bound (scalar).
    pub h2_norm: S,
    /// DARE solution matrix P (N×N).
    pub p_matrix: Matrix<S, N, N>,
}

impl<S: ControlScalar, const N: usize, const I: usize> H2Controller<S, N, I> {
    /// Construct directly from pre-computed gain and H2 norm.
    pub fn new(gain: Matrix<S, I, N>, h2_norm: S) -> Self {
        Self {
            gain,
            h2_norm,
            p_matrix: Matrix::zeros(),
        }
    }

    /// Design H2 controller from system matrices and cost weights.
    ///
    /// Solves DARE and computes H2 norm bound using the disturbance input matrix.
    /// Returns None if DARE fails.
    pub fn design(
        a: &Matrix<S, N, N>,
        b: &Matrix<S, N, I>,
        q: &Matrix<S, N, N>,
        r: &Matrix<S, I, I>,
        b_disturbance: &Matrix<S, N, I>,
    ) -> Option<Self> {
        let (gain, p) = solve_h2_dare(a, b, q, r)?;
        let norm = h2_norm_bound(&p, b_disturbance);
        Some(Self {
            gain,
            h2_norm: norm,
            p_matrix: p,
        })
    }

    /// Compute control input: u = -K*x.
    pub fn control(&self, x: &Matrix<S, N, 1>) -> Matrix<S, I, 1> {
        let kx = matmul(&self.gain, x);
        kx.neg()
    }

    /// Compute control for state given as array slice.
    pub fn control_arr(&self, x: &[S; N]) -> [S; I] {
        let xm = Matrix {
            data: core::array::from_fn(|r| [x[r]]),
        };
        let u = self.control(&xm);
        core::array::from_fn(|i| u.data[i][0])
    }

    /// Return the steady-state cost for a given initial state.
    ///
    /// The H2 cost from state x is x^T P x.
    pub fn cost_from_state(&self, x: &Matrix<S, N, 1>) -> S {
        let px = matmul(&self.p_matrix, x);
        let xt = x.transpose();
        let cost = matmul(&xt, &px);
        cost.data[0][0]
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    fn double_integrator() -> (Matrix<f64, 2, 2>, Matrix<f64, 2, 1>) {
        let mut a = Matrix::<f64, 2, 2>::identity();
        a.data[0][1] = 0.1;
        let mut b = Matrix::<f64, 2, 1>::zeros();
        b.data[0][0] = 0.005;
        b.data[1][0] = 0.1;
        (a, b)
    }

    #[test]
    fn h2_dare_converges() {
        let (a, b) = double_integrator();
        let q = Matrix::<f64, 2, 2>::identity();
        let mut r = Matrix::<f64, 1, 1>::zeros();
        r.data[0][0] = 0.1;
        let result = solve_h2_dare(&a, &b, &q, &r);
        assert!(result.is_some(), "H2 DARE should converge");
        let (k, _p) = result.unwrap();
        // Gain should be nonzero
        assert!(k.data[0][0].abs() > 0.0 || k.data[0][1].abs() > 0.0);
    }

    #[test]
    fn h2_controller_stabilizes() {
        let (a, b) = double_integrator();
        let q = Matrix::<f64, 2, 2>::identity();
        let mut r = Matrix::<f64, 1, 1>::zeros();
        r.data[0][0] = 0.1;
        let ctrl = H2Controller::design(&a, &b, &q, &r, &b).unwrap();

        // Simulate: x0 = [1, 0], should converge to 0
        let mut x = Matrix::<f64, 2, 1>::zeros();
        x.data[0][0] = 1.0;
        for _ in 0..300 {
            let u = ctrl.control(&x);
            let ax = matmul(&a, &x);
            let bu = matmul(&b, &u);
            x = ax.add_mat(&bu);
        }
        assert!(
            x.data[0][0].abs() < 0.01,
            "Position should converge: {}",
            x.data[0][0]
        );
        assert!(
            x.data[1][0].abs() < 0.01,
            "Velocity should converge: {}",
            x.data[1][0]
        );
    }

    #[test]
    fn h2_norm_bound_nonneg() {
        let (a, b) = double_integrator();
        let q = Matrix::<f64, 2, 2>::identity();
        let mut r = Matrix::<f64, 1, 1>::zeros();
        r.data[0][0] = 0.1;
        let (_k, p) = solve_h2_dare(&a, &b, &q, &r).unwrap();
        let norm = h2_norm_bound(&p, &b);
        assert!(norm >= 0.0, "H2 norm bound must be non-negative: {}", norm);
    }

    #[test]
    fn h2_cost_from_state() {
        let (a, b) = double_integrator();
        let q = Matrix::<f64, 2, 2>::identity();
        let mut r = Matrix::<f64, 1, 1>::zeros();
        r.data[0][0] = 0.1;
        let ctrl = H2Controller::design(&a, &b, &q, &r, &b).unwrap();

        let mut x0 = Matrix::<f64, 2, 1>::zeros();
        x0.data[0][0] = 1.0;
        let cost = ctrl.cost_from_state(&x0);
        assert!(
            cost > 0.0,
            "Cost from non-zero state should be positive: {}",
            cost
        );

        let x_zero = Matrix::<f64, 2, 1>::zeros();
        let cost_zero = ctrl.cost_from_state(&x_zero);
        assert!(
            cost_zero.abs() < 1e-12,
            "Cost from zero state should be zero: {}",
            cost_zero
        );
    }
}