Skip to content

Commit

Permalink
added rlorp
Browse files Browse the repository at this point in the history
  • Loading branch information
@jwhtkr
jwhtkr committed Apr 3, 2024
1 parent 592fd03 commit b731234
Show file tree
Hide file tree
Showing 4 changed files with 355 additions and 1 deletion.
1 change: 1 addition & 0 deletions src/control_theory.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,7 @@ where
let b_transpose = b_mat.t();
let r_inverse = Array2::inv(r_mat)?;

// TODO: Convert this to use one of the better integrators. Maybe RK45.
for _i in 0..iter_max {
let p_next = &p_mat
+ (p_mat.dot(a_mat) + a_transpose.dot(&p_mat)
Expand Down
1 change: 1 addition & 0 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ pub mod integrator;
pub mod control_laws;
pub mod control_theory;
pub mod lorp;
pub mod rlorp;
pub use mas::*;
pub use dynamics::LtiDynamics;
pub use integrator::EulerIntegration;
2 changes: 1 addition & 1 deletion src/lorp.rs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
//! Linear Output Regulation Problem solutions/functionality.
//! Linear Output Regulation Problem (LORP) solutions/functionality.

use ndarray::{
concatenate, linalg::kron, s, Array1, Array2, ArrayBase, Axis, Data, Ix2, LinalgScalar,
Expand Down
352 changes: 352 additions & 0 deletions src/rlorp.rs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,352 @@
//! Robust Linear Output Regulation Problem (RLORP) functionality/solutions

use ndarray::{concatenate, s, Array1, Array2, Axis, LinalgScalar};
use ndarray_linalg::Lapack;

use crate::{control_theory::lqr, dynamics::Dynamics, lorp::LorpDynamics, LtiDynamics};

/// Implement linear, time-invariant RLORP dynamics for convenience
///
/// $$ \dot{x} = (A + \Delta A) x + (B + \Delta B) u + (E + \Delta E) v $$
/// $$ e = (C + \Delta C) x + (D + \Delta D) u + (F + \Delta F) v $$
/// $$ \dot{v} = S v
pub struct RlorpDynamics<T: LinalgScalar> {
pub lorp_dynamics: LorpDynamics<T>,
pub delta_a: Array2<T>,
pub delta_b: Array2<T>,
pub delta_c: Array2<T>,
pub delta_d: Array2<T>,
pub delta_e: Array2<T>,
pub delta_f: Array2<T>,
a_tilde: Array2<T>,
b_tilde: Array2<T>,
c_tilde: Array2<T>,
d_tilde: Array2<T>,
}
impl<T: LinalgScalar> RlorpDynamics<T> {
pub fn new(
lorp_dynamics: LorpDynamics<T>,
delta_a: Array2<T>,
delta_b: Array2<T>,
delta_c: Array2<T>,
delta_d: Array2<T>,
delta_e: Array2<T>,
delta_f: Array2<T>,
) -> Self {
let a_tilde = concatenate![
Axis(0),
concatenate![
Axis(1),
(&lorp_dynamics.a_mat + &delta_a),
(&lorp_dynamics.e_mat + &delta_e)
],
concatenate![
Axis(1),
Array2::zeros((lorp_dynamics.s_mat.nrows(), lorp_dynamics.a_mat.ncols())),
lorp_dynamics.s_mat
]
];
let b_tilde = concatenate![
Axis(0),
(&lorp_dynamics.b_mat + &delta_b),
Array2::zeros((lorp_dynamics.s_mat.nrows(), lorp_dynamics.b_mat.ncols()))
];
let c_tilde = concatenate![
Axis(1),
(&lorp_dynamics.c_mat + &delta_c),
(&lorp_dynamics.f_mat + &delta_f)
];
let d_tilde = &lorp_dynamics.d_mat + &delta_d;
RlorpDynamics {
lorp_dynamics,
delta_a,
delta_b,
delta_c,
delta_d,
delta_e,
delta_f,
a_tilde,
b_tilde,
c_tilde,
d_tilde,
}
}

fn a_tilde(&self) -> &Array2<T> {
&self.a_tilde
}

fn b_tilde(&self) -> &Array2<T> {
&self.b_tilde
}

fn c_tilde(&self) -> &Array2<T> {
&self.c_tilde
}

fn d_tilde(&self) -> &Array2<T> {
&self.d_tilde
}
}
impl<T: LinalgScalar> Dynamics<T> for RlorpDynamics<T> {
fn n_input(&self) -> usize {
self.b_tilde.ncols()
}
fn n_state(&self) -> usize {
self.a_tilde.ncols()
}
fn n_output(&self) -> usize {
self.c_tilde.nrows()
}
fn dynamics(&self, _t: T, x: &Array1<T>, u: &Array1<T>) -> Array1<T> {
self.a_tilde.dot(x) + self.b_tilde.dot(u)
}
fn output(&self, _t: T, x: &Array1<T>, u: &Array1<T>) -> Array1<T> {
self.c_tilde.dot(x) + self.d_tilde.dot(u)
}
}

/// Create a block diagonal matrix.
fn block_diag<T: LinalgScalar>(blocks: &[Array2<T>]) -> Array2<T> {
let rows: Vec<_> = blocks.iter().map(|v| v.nrows()).collect();
let cols: Vec<_> = blocks.iter().map(|v| v.ncols()).collect();
let total_rows = rows.iter().sum();
let total_cols = cols.iter().sum();

let mut out_arr = Array2::zeros((total_rows, total_cols));

let mut row_start = 0;
let mut col_start = 0;
for (i, block) in blocks.iter().enumerate() {
let nrow = rows[i];
let ncol = cols[i];
out_arr
.slice_mut(s![row_start..row_start + nrow, col_start..col_start + ncol])
.assign(block);
row_start += nrow;
col_start += ncol;
}

out_arr
}

/// Create a p-copy internal model from the given minimum polynomial
fn p_copy_internal_model<T: LinalgScalar + Lapack>(
minimal_polynomial: &[T],
n_p: usize,
) -> (Array2<T>, Array2<T>) {
let n = minimal_polynomial.len() - 1;
let beta = concatenate![
Axis(0),
concatenate![Axis(1), Array2::zeros((n - 1, 1)), Array2::eye(n - 1)],
-Array2::from_shape_vec(
(1, n),
minimal_polynomial.iter().copied().skip(1).rev().collect()
)
.unwrap()
];
let sigma = Array2::from_shape_fn(
(n, 1),
|(i, _j)| if i < n - 1 { T::zero() } else { T::one() },
);

let v1_mat = block_diag(&std::iter::repeat(beta).take(n_p).collect::<Vec<_>>());
let v2_mat = block_diag(&std::iter::repeat(sigma).take(n_p).collect::<Vec<_>>());
(v1_mat, v2_mat)
}

/// Create the dynamic state feedback controller solving the RLORP
///
/// Creates an augmented dynamic system and the feedback input function based on
/// the augmented system.
/// The feedback has the form $u = K_1 x + K_2 z$ with $\dot{z} = G_1 z + G_2 e$
/// with $K_1$, $K_2$, $G_1$, and $G_2$ being the design parameters.
///
/// The $G_1$, $G_2$ are constructed to incorporate an internal model of $S$,
/// While $K_1$ and $K_2$ are constructed to stabilize the system
/// $$
/// \dot{x} = \begin{bmatrix} A & 0 \\ G_2C & G_1 \end{bmatrix}
/// + \begin{bmatrix} B \\ G_2D \end{bmatrix} u
/// $$
/// $$
/// u = \begin{bmatrix} K_1 & K_2 \end{bmatrix} x
/// $$
pub fn dynamic_state_feedback_controller_rlorp<
T: LinalgScalar + Lapack + ndarray::ScalarOperand + std::cmp::PartialOrd,
>(
rlorp_dynamics: &RlorpDynamics<T>,
min_poly_s: &[T],
q_mat: Option<&Array2<T>>,
r_mat: Option<&Array2<T>>,
) -> (LtiDynamics<T>, impl Fn(T, &Array1<T>) -> Array1<T>) {
let (g1_mat, g2_mat) =
p_copy_internal_model(min_poly_s, rlorp_dynamics.lorp_dynamics.c_mat.nrows());

let a_bar = concatenate![
Axis(0),
concatenate![
Axis(1),
rlorp_dynamics.lorp_dynamics.a_mat,
Array2::zeros((rlorp_dynamics.lorp_dynamics.a_mat.nrows(), g1_mat.ncols()))
],
concatenate![
Axis(1),
g2_mat.dot(&rlorp_dynamics.lorp_dynamics.c_mat),
g1_mat
]
];
let b_bar = concatenate![
Axis(0),
rlorp_dynamics.lorp_dynamics.b_mat,
g2_mat.dot(&rlorp_dynamics.lorp_dynamics.d_mat)
];
let k_mat = -lqr(
&a_bar,
&b_bar,
q_mat,
r_mat,
Default::default(),
Default::default(),
Default::default(),
)
.unwrap();


let k_mat_aug = concatenate![
Axis(1),
k_mat.slice(s![.., 0..rlorp_dynamics.lorp_dynamics.a_mat.ncols()]),
Array2::zeros((k_mat.nrows(), rlorp_dynamics.lorp_dynamics.s_mat.ncols())),
k_mat.slice(s![.., rlorp_dynamics.lorp_dynamics.a_mat.ncols()..])
];
println!("{k_mat_aug}");
(
augmented_system_dynamic_state_feedback(rlorp_dynamics, &g1_mat, &g2_mat),
move |_t, x| k_mat_aug.dot(x),
)
}

pub fn augmented_system_dynamic_state_feedback<T: LinalgScalar>(rlorp_dynamics: &RlorpDynamics<T>, g1_mat: &Array2<T>, g2_mat: &Array2<T>) -> LtiDynamics<T> {
let a_aug = concatenate![
Axis(0),
concatenate![
Axis(1),
*rlorp_dynamics.a_tilde(),
Array2::zeros((rlorp_dynamics.n_state(), g1_mat.ncols()))
],
concatenate![
Axis(1),
concatenate![
Axis(1),
g2_mat.dot(&(&rlorp_dynamics.lorp_dynamics.c_mat + &rlorp_dynamics.delta_c)),
g2_mat.dot(&(&rlorp_dynamics.lorp_dynamics.f_mat + &rlorp_dynamics.delta_f))
],
*g1_mat
]
];
let b_aug = concatenate![
Axis(0),
*rlorp_dynamics.b_tilde(),
g2_mat.dot(&(&rlorp_dynamics.lorp_dynamics.d_mat + &rlorp_dynamics.delta_d))
];
let c_aug = concatenate![
Axis(1),
*rlorp_dynamics.c_tilde(),
Array2::zeros((rlorp_dynamics.c_tilde().nrows(), g1_mat.ncols()))
];
let d_aug = rlorp_dynamics.d_tilde().clone();
LtiDynamics {a_mat: a_aug, b_mat: b_aug, c_mat: c_aug, d_mat: d_aug}
}

#[cfg(test)]
mod tests {
use ndarray::array;

use super::*;

#[test]
fn test_dynamic_state_feedback_controller_rlorp() {
let a_mat = array![
[-0.0158, 0.02633, -9.81, 0.],
[-0.1531, -1.03, 0., 120.5],
[0., 0., 0., 1.],
[0.0005274, -0.01652, 0., -1.466]
];
let b_mat = array![[0.0006056, 0.], [0., -9.496], [0., 0.], [0., -5.565]];
let c_mat = array![[1., 0., 0., 0.]];
let d_mat = array![[0., 0.]];
let e_mat = array![
[0., 0., 0.],
[0., 0., -9.496],
[0., 0., 0.],
[0., 0., -5.565]
];
let f_mat = array![[-1., 0., 0.]];
let s_mat = array![[0., 0., 0.], [0., 0., 1.], [0., -1., 0.]];
let delta_a = Array2::zeros(a_mat.raw_dim());
let delta_b = Array2::zeros(b_mat.raw_dim());
let delta_c = Array2::zeros(c_mat.raw_dim());
let delta_d = Array2::zeros(d_mat.raw_dim());
let delta_e = Array2::zeros(e_mat.raw_dim());
let delta_f = Array2::zeros(f_mat.raw_dim());
let lorp_dynamics = LorpDynamics::new(a_mat, b_mat, c_mat, d_mat, e_mat, f_mat, s_mat);
let rlorp_dynamics = RlorpDynamics::new(
lorp_dynamics,
delta_a,
delta_b,
delta_c,
delta_d,
delta_e,
delta_f,
);

let min_poly_s = [1., 0., 1., 0.];

let (rlorp_dyn, rlorp_ctrl) =
dynamic_state_feedback_controller_rlorp(&rlorp_dynamics, &min_poly_s, None, None);

assert!(rlorp_dyn.a_mat.abs_diff_eq(
&array![
[-1.58e-2, 2.633e-2, -9.81, 0., 0., 0., 0., 0., 0., 0.],
[-1.531e-1, -1.03, 0., 1.205e2, 0., 0., -9.496, 0., 0., 0.],
[0., 0., 0., 1., 0., 0., 0., 0., 0., 0.],
[5.274e-4, -1.652e-2, 0., -1.466, 0., 0., -5.565, 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., -1., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 1.],
[1., 0., 0., 0., -1., 0., 0., 0., -1., 0.]
],
1e-12
));
assert!(rlorp_dyn.b_mat.abs_diff_eq(
&array![
[6.056e-4, 0.],
[0., -9.496],
[0., 0.],
[0., -5.565],
[0., 0.],
[0., 0.],
[0., 0.],
[0., 0.],
[0., 0.],
[0., 0.]
],
1e-12
));
assert!(rlorp_dyn.c_mat.abs_diff_eq(&array![[1., 0., 0., 0., -1., 0., 0., 0., 0., 0.]], 1e-12));
assert!(rlorp_dyn.d_mat.abs_diff_eq(&array![[0., 0.]], 1e-12));

let eye10 = Array2::eye(10);
assert!(rlorp_ctrl(0., &eye10.slice(s![0, ..]).to_owned()).abs_diff_eq(&array![-3.513697633313342e-2, -6.335195984931541], 1e-6));
assert!(rlorp_ctrl(0., &eye10.slice(s![1, ..]).to_owned()).abs_diff_eq(&array![-1.95060633838762e-3, 2.541285622352293e-1], 1e-7));
assert!(rlorp_ctrl(0., &eye10.slice(s![2, ..]).to_owned()).abs_diff_eq(&array![2.646935642526326e-1, 9.260870385043025e1], 1e-5));
assert!(rlorp_ctrl(0., &eye10.slice(s![3, ..]).to_owned()).abs_diff_eq(&array![4.017889034646774e-3, 6.243495025478761], 1e-7));
assert!(rlorp_ctrl(0., &eye10.slice(s![4, ..]).to_owned()).abs_diff_eq(&array![0., 0.], 1e-12));
assert!(rlorp_ctrl(0., &eye10.slice(s![5, ..]).to_owned()).abs_diff_eq(&array![0., 0.], 1e-12));
assert!(rlorp_ctrl(0., &eye10.slice(s![6, ..]).to_owned()).abs_diff_eq(&array![0., 0.], 1e-12));
assert!(rlorp_ctrl(0., &eye10.slice(s![7, ..]).to_owned()).abs_diff_eq(&array![-5.717006212533678e-3, -9.999836577864565e-1], 1e-7));
assert!(rlorp_ctrl(0., &eye10.slice(s![8, ..]).to_owned()).abs_diff_eq(&array![1.191185788711509e-2, 1.717901328143854], 1e-7));
assert!(rlorp_ctrl(0., &eye10.slice(s![9, ..]).to_owned()).abs_diff_eq(&array![-1.270628456329595e-2, -1.220492805876535], 1e-7));
}
}

0 comments on commit b731234

Please sign in to comment.