pub fn solve_continuous_riccati_eigen(
    A: &DMatrix<f32>,
    B: &DMatrix<f32>,
    Q: &DMatrix<f32>,
    R: &DMatrix<f32>,
) -> Result<DMatrix<f32>, LinAlgError> {

    let dx = A.shape().0;

    let P: DMatrix<f32>;

    let Rinv = match R.clone().try_inverse() {
        Some(inverse) => inverse,
        None => return Err(LinAlgError),
    };

    let BRinvBT = B * Rinv * B.transpose(); // matrix operations perform a move
    let AT = A.transpose();

    // multiplication of DMatrix returns DMatrix, not a borrow
    // row-major listing matrices to be placed into hamiltonian
    // let matrices: [&DMatrix<f32>; 4] = [A, &BRinvBT, &(-Q), &AT];

    let upper_block = match hcombine(A, &(-BRinvBT)) {
        Ok(concatenated) => concatenated, // assign concatenated to upper block
        Err(MatrixCompareError) => return Err(LinAlgError),
    };

    let lower_block = match hcombine(&(-Q), &(-AT)) {
        Ok(concatenated) => concatenated, // assign concatenated to lower block
        Err(MatrixCompareError) => return Err(LinAlgError),
    };

    let mut hamiltonian = match vcombine(&upper_block, &lower_block) {
        Ok(concatenated) => concatenated, // assign concatenated to hamiltonian
        Err(MatrixCompareError) => return Err(LinAlgError),
    };

    // use LAPACK to compute ordered real schur decomposition of hamiltonian
    // Ordering ensures eigenvalues placed in top left of quasi-upper triangular matrix are stable / negative

    // C function to sort only the eigenvalues with a negative real part
    // "An eigenvalue wr(j)+sqrt(-1)wi(j) is selected if select(wr(j), wi(j)) is true"
    // wr_j: real part of the eigenvalue
    // wi_j: imaginary part of eigenvalue
    extern "C" fn selectfcn(wr_j: *const f32, _wi_j: *const f32) -> i32 {
        unsafe {
            match wr_j.as_ref() {
                // dereference raw pointer
                Some(val) => (*val < 0.0) as i32,
                None => 0,
            }
        }
    }

    let (nrows, _ncols) = hamiltonian.shape();
    let select: Select2F32 = Some(selectfcn);
    let lda = nrows as i32;
    let mut sdim = 0;
    // wr
    let mut wr = Vec::with_capacity(nrows);
    unsafe { wr.set_len(nrows) };
    // wi
    let mut wi = Vec::with_capacity(nrows);
    unsafe { wi.set_len(nrows) };
    // vs
    let ldvs = nrows as i32;
    let mut vs = Vec::with_capacity((ldvs as usize) * nrows);
    unsafe { vs.set_len((ldvs as usize) * nrows) };
    // rwork
    let mut work = Vec::with_capacity(1 as usize);
    unsafe { work.set_len(1 as usize) };
    // bwork
    let mut bwork = Vec::with_capacity(nrows);
    unsafe { bwork.set_len(nrows) };
    let lwork = -1;

    let mut info = 0;

    // Compute the optimal size of the workspace array
    unsafe {
        sgees(
            b'V',
            b'S',
            select,
            nrows as i32,
            hamiltonian.as_mut_slice(),
            lda,
            &mut sdim,
            &mut wr,
            &mut wi,
            &mut vs,
            ldvs,
            &mut work,
            lwork,
            &mut bwork,
            &mut info,
        );
    }

    if info == -1 {
        return Err(LinAlgError);
    }

    let lda = nrows as i32;
    let mut sdim = 0;
    let lwork = work[0] as i32; // retrieve optimal workspace size
    let mut wr = Vec::with_capacity(nrows);
    unsafe { wr.set_len(nrows) };
    let mut wi = Vec::with_capacity(nrows);
    unsafe { wi.set_len(nrows) };
    let ldvs = nrows as i32;
    let mut vs = Vec::with_capacity((ldvs as usize) * nrows);
    unsafe { vs.set_len((ldvs as usize) * nrows) };
    let mut work = Vec::with_capacity(lwork as usize);
    unsafe { work.set_len(lwork as usize) };
    let mut bwork = Vec::with_capacity(nrows);
    unsafe { bwork.set_len(nrows) };
    let mut info = 0;

    // Compute the Schur Factorization
    unsafe {
        sgees(
            b'V',
            b'S',
            select,
            nrows as i32,
            hamiltonian.as_mut_slice(),
            lda,
            &mut sdim,
            wr.as_mut_slice(),
            wi.as_mut_slice(),
            vs.as_mut_slice(),
            ldvs,
            &mut work,
            lwork,
            &mut bwork,
            &mut info,
        );
    }

    if info == -1 {
        return Err(LinAlgError);
    }

    // println!("Z"); //  Real-schur form matrix
    // print_matrix(&hamiltonian);
    // println!("T"); // unitary matrix
    let T = DMatrix::from_column_slice(nrows, _ncols, &vs);
    // print_matrix(&T);

    // println!("T Z T^T");
    // print_matrix(&(&T * &hamiltonian * &T.transpose()));

    // Extract submatrices corresponding to stable eigenvectors
    let u11 = block((0, 0), (dx - 1, dx - 1), &T);
    let u21 = block((dx, 0), (2 * dx - 1, dx - 1), &T);

    let u11_inv = match u11.clone().try_inverse() {
        Some(inverse) => inverse,
        None => return Err(LinAlgError),
    };

    P = u21 * u11_inv;

    Ok(P)
}
/// Solve algebraic riccati equation using Hamiltonian eigenvalue decomposition
/// Reference: "A Schur Method for Solving Algebraic Riccati Equations" - Alan J. Laub
Solve algebraic riccati equation using Hamiltonian eigenvalue decomposition
Reference: "A Schur Method for Solving Algebraic Riccati Equations" - Alan J. Laub
Solve algebraic riccati equation using Hamiltonian eigenvalue decomposition Reference : " A Schur Method for Solving Algebraic Riccati Equations " - Alan J . Laub
pub fn solve_continuous_riccati_eigen(
    A: &DMatrix<f32>,
    B: &DMatrix<f32>,
    Q: &DMatrix<f32>,
    R: &DMatrix<f32>,
) -> Result<DMatrix<f32>, LinAlgError> {
    let dx = A.shape().0;
    let P: DMatrix<f32>;
    let Rinv = match R.clone().try_inverse() {
        Some(inverse) => inverse,
        None => return Err(LinAlgError),
    };
    let BRinvBT = B * Rinv * B.transpose(); // matrix operations perform a move
    let AT = A.transpose();
    // multiplication of DMatrix returns DMatrix, not a borrow
    // row-major listing matrices to be placed into hamiltonian
    // let matrices: [&DMatrix<f32>; 4] = [A, &BRinvBT, &(-Q), &AT];
    let upper_block = match hcombine(A, &(-BRinvBT)) {
        Ok(concatenated) => concatenated, // assign concatenated to upper block
        Err(MatrixCompareError) => return Err(LinAlgError),
    };
    let lower_block = match hcombine(&(-Q), &(-AT)) {
        Ok(concatenated) => concatenated, // assign concatenated to lower block
        Err(MatrixCompareError) => return Err(LinAlgError),
    };
    let mut hamiltonian = match vcombine(&upper_block, &lower_block) {
        Ok(concatenated) => concatenated, // assign concatenated to hamiltonian
        Err(MatrixCompareError) => return Err(LinAlgError),
    };
    // use LAPACK to compute ordered real schur decomposition of hamiltonian
    // Ordering ensures eigenvalues placed in top left of quasi-upper triangular matrix are stable / negative
    // C function to sort only the eigenvalues with a negative real part
    // "An eigenvalue wr(j)+sqrt(-1)wi(j) is selected if select(wr(j), wi(j)) is true"
    // wr_j: real part of the eigenvalue
    // wi_j: imaginary part of eigenvalue
    extern "C" fn selectfcn(wr_j: *const f32, _wi_j: *const f32) -> i32 {
        unsafe {
            match wr_j.as_ref() {
                // dereference raw pointer
                Some(val) => (*val < 0.0) as i32,
                None => 0,
            }
        }
    }
    let (nrows, _ncols) = hamiltonian.shape();
    let select: Select2F32 = Some(selectfcn);
    let lda = nrows as i32;
    let mut sdim = 0;
    // wr
    let mut wr = Vec::with_capacity(nrows);
    unsafe { wr.set_len(nrows) };
    // wi
    let mut wi = Vec::with_capacity(nrows);
    unsafe { wi.set_len(nrows) };
    // vs
    let ldvs = nrows as i32;
    let mut vs = Vec::with_capacity((ldvs as usize) * nrows);
    unsafe { vs.set_len((ldvs as usize) * nrows) };
    // rwork
    let mut work = Vec::with_capacity(1 as usize);
    unsafe { work.set_len(1 as usize) };
    // bwork
    let mut bwork = Vec::with_capacity(nrows);
    unsafe { bwork.set_len(nrows) };
    let lwork = -1;
    let mut info = 0;
    // Compute the optimal size of the workspace array
    unsafe {
        sgees(
            b'V',
            b'S',
            select,
            nrows as i32,
            hamiltonian.as_mut_slice(),
            lda,
            &mut sdim,
            &mut wr,
            &mut wi,
            &mut vs,
            ldvs,
            &mut work,
            lwork,
            &mut bwork,
            &mut info,
        );
    }
    if info == -1 {
        return Err(LinAlgError);
    }
    let lda = nrows as i32;
    let mut sdim = 0;
    let lwork = work[0] as i32; // retrieve optimal workspace size
    let mut wr = Vec::with_capacity(nrows);
    unsafe { wr.set_len(nrows) };
    let mut wi = Vec::with_capacity(nrows);
    unsafe { wi.set_len(nrows) };
    let ldvs = nrows as i32;
    let mut vs = Vec::with_capacity((ldvs as usize) * nrows);
    unsafe { vs.set_len((ldvs as usize) * nrows) };
    let mut work = Vec::with_capacity(lwork as usize);
    unsafe { work.set_len(lwork as usize) };
    let mut bwork = Vec::with_capacity(nrows);
    unsafe { bwork.set_len(nrows) };
    let mut info = 0;
    // Compute the Schur Factorization
    unsafe {
        sgees(
            b'V',
            b'S',
            select,
            nrows as i32,
            hamiltonian.as_mut_slice(),
            lda,
            &mut sdim,
            wr.as_mut_slice(),
            wi.as_mut_slice(),
            vs.as_mut_slice(),
            ldvs,
            &mut work,
            lwork,
            &mut bwork,
            &mut info,
        );
    }
    if info == -1 {
        return Err(LinAlgError);
    }
    // println!("Z"); //  Real-schur form matrix
    // print_matrix(&hamiltonian);
    // println!("T"); // unitary matrix
    let T = DMatrix::from_column_slice(nrows, _ncols, &vs);
    // print_matrix(&T);
    // println!("T Z T^T");
    // print_matrix(&(&T * &hamiltonian * &T.transpose()));
    // Extract submatrices corresponding to stable eigenvectors
    let u11 = block((0, 0), (dx - 1, dx - 1), &T);
    let u21 = block((dx, 0), (2 * dx - 1, dx - 1), &T);
    let u11_inv = match u11.clone().try_inverse() {
        Some(inverse) => inverse,
        None => return Err(LinAlgError),
    };
    P = u21 * u11_inv;
    Ok(P)
}
pub fn solve_continuous_riccati_eigen ( A : & DMatrix < f32 > , B : & DMatrix < f32 > , Q : & DMatrix < f32 > , R : & DMatrix < f32 > , ) -> Result < DMatrix < f32 > , LinAlgError > { let dx = A . shape ( ) . 0 ; let P : DMatrix < f32 > ; let Rinv = match R . clone ( ) . try_inverse ( ) { Some ( inverse ) => inverse , None => return Err ( LinAlgError ) , } ; let BRinvBT = B * Rinv * B . transpose ( ) ; // matrix operations perform a move let AT = A . transpose ( ) ; // multiplication of DMatrix returns DMatrix, not a borrow // row-major listing matrices to be placed into hamiltonian // let matrices: [&DMatrix<f32>; 4] = [A, &BRinvBT, &(-Q), &AT]; let upper_block = match hcombine ( A , & ( - BRinvBT ) ) { Ok ( concatenated ) => concatenated , // assign concatenated to upper block Err ( MatrixCompareError ) => return Err ( LinAlgError ) , } ; let lower_block = match hcombine ( & ( - Q ) , & ( - AT ) ) { Ok ( concatenated ) => concatenated , // assign concatenated to lower block Err ( MatrixCompareError ) => return Err ( LinAlgError ) , } ; let mut hamiltonian = match vcombine ( & upper_block , & lower_block ) { Ok ( concatenated ) => concatenated , // assign concatenated to hamiltonian Err ( MatrixCompareError ) => return Err ( LinAlgError ) , } ; // use LAPACK to compute ordered real schur decomposition of hamiltonian // Ordering ensures eigenvalues placed in top left of quasi-upper triangular matrix are stable / negative // C function to sort only the eigenvalues with a negative real part // "An eigenvalue wr(j)+sqrt(-1)wi(j) is selected if select(wr(j), wi(j)) is true" // wr_j: real part of the eigenvalue // wi_j: imaginary part of eigenvalue extern " " fn selectfcn ( wr_j : * const f32 , _wi_j : * const f32 ) -> i32 { unsafe { match wr_j . as_ref ( ) { // dereference raw pointer Some ( val ) => ( * val < 0.0 ) as i32 , None => 0 , } } } let ( nrows , _ncols ) = hamiltonian . shape ( ) ; let select : Select2F32 = Some ( selectfcn ) ; let lda = nrows as i32 ; let mut sdim = 0 ; // wr let mut wr = Vec :: with_capacity ( nrows ) ; unsafe { wr . set_len ( nrows ) } ; // wi let mut wi = Vec :: with_capacity ( nrows ) ; unsafe { wi . set_len ( nrows ) } ; // vs let ldvs = nrows as i32 ; let mut vs = Vec :: with_capacity ( ( ldvs as usize ) * nrows ) ; unsafe { vs . set_len ( ( ldvs as usize ) * nrows ) } ; // rwork let mut work = Vec :: with_capacity ( 1 as usize ) ; unsafe { work . set_len ( 1 as usize ) } ; // bwork let mut bwork = Vec :: with_capacity ( nrows ) ; unsafe { bwork . set_len ( nrows ) } ; let lwork = - 1 ; let mut info = 0 ; // Compute the optimal size of the workspace array unsafe { sgees ( b'V' , b'S' , select , nrows as i32 , hamiltonian . as_mut_slice ( ) , lda , & mut sdim , & mut wr , & mut wi , & mut vs , ldvs , & mut work , lwork , & mut bwork , & mut info , ) ; } if info == - 1 { return Err ( LinAlgError ) ; } let lda = nrows as i32 ; let mut sdim = 0 ; let lwork = work [ 0 ] as i32 ; // retrieve optimal workspace size let mut wr = Vec :: with_capacity ( nrows ) ; unsafe { wr . set_len ( nrows ) } ; let mut wi = Vec :: with_capacity ( nrows ) ; unsafe { wi . set_len ( nrows ) } ; let ldvs = nrows as i32 ; let mut vs = Vec :: with_capacity ( ( ldvs as usize ) * nrows ) ; unsafe { vs . set_len ( ( ldvs as usize ) * nrows ) } ; let mut work = Vec :: with_capacity ( lwork as usize ) ; unsafe { work . set_len ( lwork as usize ) } ; let mut bwork = Vec :: with_capacity ( nrows ) ; unsafe { bwork . set_len ( nrows ) } ; let mut info = 0 ; // Compute the Schur Factorization unsafe { sgees ( b'V' , b'S' , select , nrows as i32 , hamiltonian . as_mut_slice ( ) , lda , & mut sdim , wr . as_mut_slice ( ) , wi . as_mut_slice ( ) , vs . as_mut_slice ( ) , ldvs , & mut work , lwork , & mut bwork , & mut info , ) ; } if info == - 1 { return Err ( LinAlgError ) ; } // println!("Z"); //  Real-schur form matrix // print_matrix(&hamiltonian); // println!("T"); // unitary matrix let T = DMatrix :: from_column_slice ( nrows , _ncols , & vs ) ; // print_matrix(&T); // println!("T Z T^T"); // print_matrix(&(&T * &hamiltonian * &T.transpose())); // Extract submatrices corresponding to stable eigenvectors let u11 = block ( ( 0 , 0 ) , ( dx - 1 , dx - 1 ) , & T ) ; let u21 = block ( ( dx , 0 ) , ( 2 * dx - 1 , dx - 1 ) , & T ) ; let u11_inv = match u11 . clone ( ) . try_inverse ( ) { Some ( inverse ) => inverse , None => return Err ( LinAlgError ) , } ; P = u21 * u11_inv ; Ok ( P ) }
Solve algebraic riccati equation using Hamiltonian eigenvalue decomposition
Reference: "A Schur Method for Solving Algebraic Riccati Equations" - Alan J. Laub
Solve algebraic riccati equation using Hamiltonian eigenvalue decomposition Reference : " A Schur Method for Solving Algebraic Riccati Equations " - Alan J . Laub
A
&DMatrix<f32>
B
&DMatrix<f32>
Q
&DMatrix<f32>
R
&DMatrix<f32>
A
&DMatrix<f32>
B
&DMatrix<f32>
Q
&DMatrix<f32>
R
&DMatrix<f32>