state space getters A(), B() .. are implemented.

Author: boyali

control/autoware_control_toolbox/include/utils_act/state_space.hpp

@@ -79,6 +79,46 @@ namespace ns_control_toolbox
 
 		[[nodiscard]] ss_system get_ssABCD_discrete() const;
 
+		[[nodiscard]] Eigen::MatrixXd Ad() const
+		{
+			return Ad_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd Bd() const
+		{
+			return Bd_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd Cd() const
+		{
+			return Cd_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd Dd() const
+		{
+			return Dd_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd A() const
+		{
+			return A_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd B() const
+		{
+			return B_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd C() const
+		{
+			return C_;
+		}
+
+		[[nodiscard]] Eigen::MatrixXd D() const
+		{
+			return D_;
+		}
+
 
 	private:
 

control/autoware_control_toolbox/usage_examples/main_balance.cpp

@@ -5,43 +5,43 @@
 #include <vector>
 
 int main()
-	{
-		
-		double Td    = 0.11; // time delay in seconds.
-		size_t order = 3; // order of the Pade approximation.
-		
-		auto tf_delay = ns_control_toolbox::pade(Td, order);
-		tf_delay.print();
-		
-		// Define Ts
-		double Ts{ 0.1 };
-		auto   ss_sys = ns_control_toolbox::tf2ss(tf_delay);
-		ss_sys.print();
-		
-		// Test discretization and compare with Matlab.
-		
-		ss_sys.discretisize(Ts);
-		ss_sys.print_discrete_system();
+{
+
+	double Td = 0.11; // time delay in seconds.
+	size_t order = 3; // order of the Pade approximation.
+
+	auto tf_delay = ns_control_toolbox::pade(Td, order);
+	tf_delay.print();
+
+	// Define Ts
+	double Ts{ 0.1 };
+	auto ss_sys = ns_control_toolbox::tf2ss(tf_delay);
+	ss_sys.print();
+
+	// Test discretization and compare with Matlab.
+
+	ss_sys.discretisize(Ts);
+	ss_sys.print_discrete_system();
 
 //	ns_control_toolbox::balance(ss_sys.A_);
 //	ns_utils::print("Balanced matrix A_");
 //	ns_eigen_utils::printEigenMat(ss_sys.A_);
-		
-		// Concat
-		auto AB        = ns_eigen_utils::hstack<double>(ss_sys.A_, ss_sys.B_);
-		auto CD        = ns_eigen_utils::hstack<double>(ss_sys.C_, ss_sys.D_);
-		auto ss_system = ns_eigen_utils::vstack<double>(AB, CD);
-		
-		ns_utils::print("Matrix to be balanced ");
-		ns_eigen_utils::printEigenMat(ss_system);
-		
-		ns_utils::print("Balanced matrix for the system ");
-		ns_control_toolbox::balance(ss_system);
-		ns_eigen_utils::printEigenMat(ss_system);
-		
-		// PERMUTATION TEST
-		
-		
-		
-		return 0;
-	}
+
+	// Concat
+	auto AB = ns_eigen_utils::hstack<double>(ss_sys.A(), ss_sys.B());
+	auto CD = ns_eigen_utils::hstack<double>(ss_sys.C(), ss_sys.D());
+	auto ss_system = ns_eigen_utils::vstack<double>(AB, CD);
+
+	ns_utils::print("Matrix to be balanced ");
+	ns_eigen_utils::printEigenMat(ss_system);
+
+	ns_utils::print("Balanced matrix for the system ");
+	ns_control_toolbox::balance(ss_system);
+	ns_eigen_utils::printEigenMat(ss_system);
+
+	// PERMUTATION TEST
+
+
+
+	return 0;
+}

control/autoware_control_toolbox/usage_examples/main_tf2ss.cpp

@@ -35,10 +35,10 @@ int main()
 	sys_ss2.print();
 
 	ns_utils::print("SS.A \n");
-	ns_eigen_utils::printEigenMat(sys_ss2.A_);
+	ns_eigen_utils::printEigenMat(sys_ss2.A());
 
 	ns_utils::print("SS.Ad \n");
-	ns_eigen_utils::printEigenMat(sys_ss2.Ad_);
+	ns_eigen_utils::printEigenMat(sys_ss2.Ad());
 
 	// DISCRETIZATION : Note sstf2 automatically discretisize
 	double Ts{ 0.1 };