void lqrCreateSparseMatrix(const LqrProblemTpl< Scalar > &problem, const Scalar mueq, Eigen::SparseMatrix< Scalar > &mat, Eigen::Matrix< Scalar, -1, 1 > &rhs, bool update)
std::array< Scalar, 3 > lqrComputeKktError(const LqrProblemTpl< Scalar > &problem, boost::span< const typename math_types< Scalar >::VectorXs > xs, boost::span< const typename math_types< Scalar >::VectorXs > us, boost::span< const typename math_types< Scalar >::VectorXs > vs, boost::span< const typename math_types< Scalar >::VectorXs > lbdas, const Scalar mueq, const std::optional< typename math_types< Scalar >::ConstVectorRef > &theta=std::nullopt, bool verbose=false)