pub fn solve_discrete_riccati_iterative(
    A: &DMatrix<f32>,
    B: &DMatrix<f32>,
    Q: &DMatrix<f32>,
    R: &DMatrix<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();

    let mut diff: f32;
    for i in 0..iter_max {
        let mut RBTPBinv = (R + &BT * &P * B).clone();
        RBTPBinv.try_inverse_mut();

        P_next = &AT * &P * A - &AT * &P * B * RBTPBinv * &BT * &P * A + Q;

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

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

    Ok(P)
}
/// Iterative solver for Algebraic Riccati Equation for discrete models
Iterative solver for Algebraic Riccati Equation for discrete models
Iterative solver for Algebraic Riccati Equation for discrete models
pub fn solve_discrete_riccati_iterative(
    A: &DMatrix<f32>,
    B: &DMatrix<f32>,
    Q: &DMatrix<f32>,
    R: &DMatrix<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();
    let mut diff: f32;
    for i in 0..iter_max {
        let mut RBTPBinv = (R + &BT * &P * B).clone();
        RBTPBinv.try_inverse_mut();
        P_next = &AT * &P * A - &AT * &P * B * RBTPBinv * &BT * &P * A + Q;
        diff = (&P_next - &P).amax();
        P = P_next;
        if diff < tolerance {
            println!("{:?}", i);
            break;
        }
    }
    Ok(P)
}
pub fn solve_discrete_riccati_iterative ( A : & DMatrix < f32 > , B : & DMatrix < f32 > , Q : & DMatrix < f32 > , R : & DMatrix < 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 ( ) ; let mut diff : f32 ; for i in 0 .. iter_max { let mut RBTPBinv = ( R + & BT * & P * B ) . clone ( ) ; RBTPBinv . try_inverse_mut ( ) ; P_next = & AT * & P * A - & AT * & P * B * RBTPBinv * & BT * & P * A + Q ; diff = ( & P_next - & P ) . amax ( ) ; P = P_next ; if diff < tolerance { println ! ( " " i ) ; break ; } } Ok ( P ) }
Iterative solver for Algebraic Riccati Equation for discrete models
Iterative solver for Algebraic Riccati Equation for discrete models
A
&DMatrix<f32>
B
&DMatrix<f32>
Q
&DMatrix<f32>
R
&DMatrix<f32>
iter_max
u32
tolerance
f32
A
&DMatrix<f32>
B
&DMatrix<f32>
Q
&DMatrix<f32>
R
&DMatrix<f32>
iter_max
u32
tolerance
f32