pub fn solve_continuous_riccati_iterative(
    A: &DMatrix<f32>,
    B: &DMatrix<f32>,
    Q: &DMatrix<f32>,
    R: &DMatrix<f32>,
    dt: f32,
    iter_max: u32,
    tolerance: f32,
) -> Result<DMatrix<f32>, LinAlgError> {

    let mut P = Q.clone_owned();

    let mut P_next: DMatrix<f32>;

    let AT = A.transpose();
    let BT = B.transpose();

    if let Some(Rinv) = R.clone().try_inverse() {
        let mut diff: f32;
        for i in 0..iter_max {
            P_next = &P + ((&P * A) + (&AT * &P) - (&P * B * &Rinv * &BT * &P) + Q) * dt;

            diff = (&P_next - &P).amax();
            P = P_next;

            if diff < tolerance {
                println!("{:?}", i);
                break;
            }
        }

        Ok(P)
    } else {
        return Err(LinAlgError);
    }
}
/// Iterative solver for Algebraic Riccati Equation for continuous models
Iterative solver for Algebraic Riccati Equation for continuous models
Iterative solver for Algebraic Riccati Equation for continuous models
pub fn solve_continuous_riccati_iterative(
    A: &DMatrix<f32>,
    B: &DMatrix<f32>,
    Q: &DMatrix<f32>,
    R: &DMatrix<f32>,
    dt: f32,
    iter_max: u32,
    tolerance: f32,
) -> Result<DMatrix<f32>, LinAlgError> {
    let mut P = Q.clone_owned();
    let mut P_next: DMatrix<f32>;
    let AT = A.transpose();
    let BT = B.transpose();
    if let Some(Rinv) = R.clone().try_inverse() {
        let mut diff: f32;
        for i in 0..iter_max {
            P_next = &P + ((&P * A) + (&AT * &P) - (&P * B * &Rinv * &BT * &P) + Q) * dt;
            diff = (&P_next - &P).amax();
            P = P_next;
            if diff < tolerance {
                println!("{:?}", i);
                break;
            }
        }
        Ok(P)
    } else {
        return Err(LinAlgError);
    }
}
pub fn solve_continuous_riccati_iterative ( A : & DMatrix < f32 > , B : & DMatrix < f32 > , Q : & DMatrix < f32 > , R : & DMatrix < f32 > , dt : f32 , iter_max : u32 , tolerance : f32 , ) -> Result < DMatrix < f32 > , LinAlgError > { let mut P = Q . clone_owned ( ) ; let mut P_next : DMatrix < f32 > ; let AT = A . transpose ( ) ; let BT = B . transpose ( ) ; if let Some ( Rinv ) = R . clone ( ) . try_inverse ( ) { let mut diff : f32 ; for i in 0 .. iter_max { P_next = & P + ( ( & P * A ) + ( & AT * & P ) - ( & P * B * & Rinv * & BT * & P ) + Q ) * dt ; diff = ( & P_next - & P ) . amax ( ) ; P = P_next ; if diff < tolerance { println ! ( " " i ) ; break ; } } Ok ( P ) } else { return Err ( LinAlgError ) ; } }
Iterative solver for Algebraic Riccati Equation for continuous models
Iterative solver for Algebraic Riccati Equation for continuous models
A
&DMatrix<f32>
B
&DMatrix<f32>
Q
&DMatrix<f32>
R
&DMatrix<f32>
dt
f32
iter_max
u32
tolerance
f32
A
&DMatrix<f32>
B
&DMatrix<f32>
Q
&DMatrix<f32>
R
&DMatrix<f32>
dt
f32
iter_max
u32
tolerance
f32