use crseo::{cu::CuType, cu::Single, Cu};
fn test_algebra() {
let x: Cu<Single> = vec![1f32, 2f32, 3f32].into();
let a_mat: Cu<Single> = vec![vec![1f32, 2f32]; 3].into();
let mut y = (&a_mat) * (&x);
println!("y: {:?}", Vec::<f32>::from(&mut y));
let mut yscale = (&y) * 2f32;
println!("yscale: {:?}", Vec::<f32>::from(&mut yscale));
yscale -= (&a_mat) * (&x);
println!("yscale: {:?}", Vec::<f32>::from(&mut yscale));
}
struct StateSpace<T: CuType> {
a_mat: Cu<T>,
b_mat: Cu<T>,
c_mat: Cu<T>,
d_mat: Option<Cu<T>>,
x: Cu<T>,
x_next: Cu<T>,
pub y: Cu<T>,
}
impl StateSpace<Single> {
pub fn new(
a_mat: Vec<Vec<f32>>,
b_mat: Vec<Vec<f32>>,
c_mat: Vec<Vec<f32>>,
d_mat: Option<Vec<Vec<f32>>>,
) -> Self {
let n_x = a_mat[0].len();
let n_y = c_mat[0].len();
Self {
a_mat: a_mat.into(),
b_mat: b_mat.into(),
c_mat: c_mat.into(),
d_mat: d_mat.map(|x| x.into()),
x: vec![0.0; n_x].into(),
x_next: vec![0.0; n_x].into(),
y: vec![0.0; n_y].into(),
}
}
pub fn update(&mut self, u: &Cu<Single>) -> &mut Cu<Single> {
self.y = &self.c_mat * &self.x;
if let Some(ref d_mat) = self.d_mat {
self.y = d_mat * u;
}
self.x_next = &self.a_mat * &self.x;
self.x_next += &self.b_mat * u;
self.x = self.x_next.clone();
&mut self.y
}
}
fn main() {
test_algebra();
let mut ss: StateSpace<Single> = StateSpace::new(
vec![vec![1f32]],
vec![vec![1f32]],
vec![vec![-0.5f32]],
None,
);
(0..50).for_each(|k| {
let mut u = Cu::<Single>::from(vec![1f32]);
u += ss.y.clone();
println!("{:2}: {:.3}", k, Vec::<f32>::from(ss.update(&u))[0]);
});
}