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

//! MPC double integrator validation: comparison with analytical solution.

use oxictl::core::matrix::Matrix;
use oxictl::mpc::linear_mpc::{LinearMpc, MpcConstraints};

fn build_double_integrator(dt: f64) -> LinearMpc<f64, 2, 1, 10> {
    let a = Matrix {
        data: [[1.0f64, dt], [0.0, 1.0]],
    };
    let b = Matrix {
        data: [[0.5 * dt * dt], [dt]],
    };
    let q = Matrix {
        data: [[1.0f64, 0.0], [0.0, 0.1]],
    };
    let q_f = Matrix {
        data: [[10.0f64, 0.0], [0.0, 1.0]],
    };
    let r = Matrix { data: [[0.01f64]] };
    let constraints = MpcConstraints::unconstrained();
    LinearMpc::new(a, b, q, q_f, r, constraints).with_optimizer(0.01, 100, 1e-6)
}

/// MPC drives double integrator from x0 to origin.
#[test]
fn double_integrator_reaches_origin() {
    let dt = 0.1f64;
    let mut mpc = build_double_integrator(dt);

    let mut x = [1.0f64, 0.0];
    let x_ref = [0.0f64, 0.0];

    for _ in 0..100 {
        let (u, _) = mpc.solve(&x, &x_ref);
        let x0 = x[0] + dt * x[1] + 0.5 * dt * dt * u[0];
        let x1 = x[1] + dt * u[0];
        x = [x0, x1];
    }

    assert!(x[0].abs() < 0.1, "Position should reach ~0: x={:.4}", x[0]);
    assert!(x[1].abs() < 0.1, "Velocity should reach ~0: v={:.4}", x[1]);
}

/// MPC returns finite control for zero initial condition.
#[test]
fn zero_initial_condition_gives_zero_control() {
    let dt = 0.1f64;
    let mut mpc = build_double_integrator(dt);
    let x0 = [0.0f64, 0.0];
    let x_ref = [0.0f64, 0.0];
    let (u, _) = mpc.solve(&x0, &x_ref);
    assert!(u[0].abs() < 0.1, "Zero IC → ~zero control: u={:.6}", u[0]);
}

/// MPC control sequence has finite length H.
#[test]
fn control_sequence_has_correct_horizon() {
    let dt = 0.1f64;
    let mut mpc = build_double_integrator(dt);
    let x0 = [1.0f64, 0.5];
    let _ = mpc.solve(&x0, &[0.0, 0.0]);
    let seq = mpc.control_sequence();
    assert_eq!(seq.len(), 10, "Horizon should be 10");
}

/// MPC with box constraints respects input limits.
#[test]
fn mpc_respects_input_constraints() {
    let dt = 0.1f64;
    let a = Matrix {
        data: [[1.0f64, dt], [0.0, 1.0]],
    };
    let b = Matrix {
        data: [[0.5 * dt * dt], [dt]],
    };
    let q = Matrix {
        data: [[1.0f64, 0.0], [0.0, 0.1]],
    };
    let q_f = Matrix {
        data: [[5.0f64, 0.0], [0.0, 0.5]],
    };
    let r = Matrix { data: [[0.01f64]] };
    let constraints = MpcConstraints::<f64, 1>::box_input([-1.0], [1.0]);
    let mut mpc = LinearMpc::<f64, 2, 1, 10>::new(a, b, q, q_f, r, constraints)
        .with_optimizer(0.01, 100, 1e-6);

    let x0 = [5.0f64, 3.0];
    let (u, _) = mpc.solve(&x0, &[0.0, 0.0]);
    assert!(
        u[0] >= -1.01 && u[0] <= 1.01,
        "Control must respect bounds: u={:.4}",
        u[0]
    );

    let seq = mpc.control_sequence();
    for &ui in seq {
        assert!(ui[0] >= -1.01 && ui[0] <= 1.01, "Sequence: u={:.4}", ui[0]);
    }
}

/// MPC cost decreases as horizon extends (more lookahead helps).
#[test]
fn mpc_converges_over_simulation() {
    let dt = 0.05f64;
    let a = Matrix {
        data: [[1.0f64, dt], [0.0, 1.0]],
    };
    let b = Matrix {
        data: [[0.5 * dt * dt], [dt]],
    };
    let q = Matrix {
        data: [[1.0f64, 0.0], [0.0, 0.1]],
    };
    let q_f = Matrix {
        data: [[10.0f64, 0.0], [0.0, 1.0]],
    };
    let r = Matrix { data: [[0.01f64]] };
    let constraints = MpcConstraints::unconstrained();
    let mut mpc =
        LinearMpc::<f64, 2, 1, 8>::new(a, b, q, q_f, r, constraints).with_optimizer(0.01, 80, 1e-5);

    let mut x = [2.0f64, 1.0];
    let x_ref = [0.0f64, 0.0];

    let initial_norm = (x[0] * x[0] + x[1] * x[1]).sqrt();

    for _ in 0..150 {
        let (u, _) = mpc.solve(&x, &x_ref);
        let x0 = x[0] + dt * x[1] + 0.5 * dt * dt * u[0];
        let x1 = x[1] + dt * u[0];
        x = [x0, x1];
    }

    let final_norm = (x[0] * x[0] + x[1] * x[1]).sqrt();
    assert!(
        final_norm < initial_norm * 0.1,
        "MPC should reduce state norm: init={:.4}, final={:.4}",
        initial_norm,
        final_norm
    );
}