nabled_sim/
control_loop.rs

1//! Closed-loop LQR + Luenberger observer step (no sensor crate dependency).
2
3use nabled_control::lqr::{LqrResult, discrete_lqr};
4use nabled_control::observer::luenberger_gain;
5use nabled_core::scalar::NabledReal;
6use nabled_linalg::lu::LuProviderScalar;
7use ndarray::{Array1, Array2};
8
9use crate::SimError;
10
11/// LTI plant matrices for closed-loop regulation.
12#[derive(Debug, Clone, PartialEq)]
13pub struct ClosedLoopPlant<T> {
14    pub a: Array2<T>,
15    pub b: Array2<T>,
16    pub c: Array2<T>,
17}
18
19/// Precomputed LQR and observer gains.
20#[derive(Debug, Clone, PartialEq)]
21pub struct ClosedLoopGains<T> {
22    pub k: Array2<T>,
23    pub l: Array2<T>,
24}
25
26/// Plant and estimated state.
27#[derive(Debug, Clone, PartialEq)]
28pub struct ClosedLoopState<T> {
29    pub x:     Array1<T>,
30    pub x_hat: Array1<T>,
31}
32
33/// One closed-loop regulation step with partial state measurement.
34#[derive(Debug, Clone, PartialEq)]
35pub struct ClosedLoopStep<T> {
36    pub plant: ClosedLoopPlant<T>,
37    pub gains: ClosedLoopGains<T>,
38}
39
40impl<T: NabledReal + LuProviderScalar> ClosedLoopStep<T> {
41    /// Design LQR gain and Luenberger observer from cost matrices and observer poles.
42    pub fn design(
43        plant: ClosedLoopPlant<T>,
44        q_cost: &Array2<T>,
45        r_cost: &Array2<T>,
46        observer_poles: &[T],
47    ) -> Result<Self, SimError> {
48        let LqrResult { gain: k, .. } = discrete_lqr(&plant.a, &plant.b, q_cost, r_cost)?;
49        let l = luenberger_gain(&plant.a, &plant.c, observer_poles)?;
50        Ok(Self { plant, gains: ClosedLoopGains { k, l } })
51    }
52
53    /// Advance plant and observer by one step; returns control input `u`.
54    pub fn step(&self, state: &mut ClosedLoopState<T>) -> Result<T, SimError> {
55        let y = self.plant.c.dot(&state.x);
56        let u = -self.gains.k.dot(&state.x_hat)[0];
57        let innovation = &y - &self.plant.c.dot(&state.x_hat);
58        state.x = self.plant.a.dot(&state.x) + &(self.plant.b.column(0).to_owned() * u);
59        state.x_hat = self.plant.a.dot(&state.x_hat)
60            + &(self.plant.b.column(0).to_owned() * u)
61            + &(self.gains.l.column(0).to_owned() * innovation[[0]]);
62        Ok(u)
63    }
64}
65
66#[cfg(test)]
67mod tests {
68    use approx::assert_relative_eq;
69    use ndarray::{arr1, arr2};
70
71    use super::*;
72
73    #[test]
74    fn closed_loop_reduces_observer_error() {
75        let dt = 0.05_f64;
76        let plant = ClosedLoopPlant {
77            a: arr2(&[[1.0, dt], [0.0, 1.0]]),
78            b: arr2(&[[0.0], [dt]]),
79            c: arr2(&[[1.0, 0.0]]),
80        };
81        let controller =
82            ClosedLoopStep::design(plant, &arr2(&[[10.0, 0.0], [0.0, 1.0]]), &arr2(&[[0.1]]), &[
83                -0.5, -0.6,
84            ])
85            .expect("design");
86        let mut state = ClosedLoopState { x: arr1(&[1.0, 0.5]), x_hat: arr1(&[0.0, 0.0]) };
87        for _ in 0..80 {
88            let _ = controller.step(&mut state).expect("step");
89        }
90        let err = (&state.x - &state.x_hat).mapv(|v: f64| v * v).sum().sqrt();
91        assert!(err < 1e-2, "observer error {err}");
92        assert_relative_eq!(state.x[0], 0.0, epsilon = 0.15);
93    }
94}
nabled_sim/control_loop.rs

nabled_sim/
control_loop.rs
