Balancing code block is changed.

control/autoware_control_toolbox/src/utils_act/balance.cpp

@@ -39,75 +39,50 @@ void ns_control_toolbox::permute(Eigen::MatrixXd& P)
 void ns_control_toolbox::balance(Eigen::MatrixXd& A)
 {
 
-	// get the size of the matrix.
-	auto const& nx = A.rows();
-	bool converged{ false };
+	const int p = 1;
+	bool converged = false;
 
-
-	while (!converged)
+	do
 	{
 		converged = true;
-
-		for (auto k = 1; k < nx; ++k)
+		for (Eigen::Index k = 0; k < A.rows(); ++k)
 		{
-
-			// Compute row and column norms.
-			auto r = A.row(k).lpNorm<1>(); // instead of 1-norm, one can use other norms as well.
-			auto c = A.col(k).lpNorm<1>();
-
-			// In the lapack implementation diagonal element is ignored in the norms.
-			// r = r - std::abs(A.row(k)(k));
-			// c = c - std::abs(A.col(k)(k));
-
-			// auto s = std::pow(r, 1) + std::pow(c, 1);
-			double f{ 1. };
+			double c = A.col(k).lpNorm<p>();
+			double r = A.row(k).lpNorm<p>();
+			double s = std::pow(c, p) + std::pow(r, p);
+			double f = 1;
 
 			if ((r > EPS) && (c > EPS))
 			{
+				// In the lapack implementation diagonal element is ignored in the norms.
 
-				auto g = r / RADIX;
-				auto&& s = r + c;
 
-				while (c < g)
+				while (c < r / RADIX)
 				{
-					f = f * RADIX;
-					c = c * RADIX;
-					r = r / RADIX;
-					g = g / RADIX;
+					c *= RADIX;
+					r /= RADIX;
+					f *= RADIX;
 				}
-
-				g = c / RADIX;
-				while (g > r)
+				while (c >= r * RADIX)
 				{
-					f = f / RADIX;
-					c = c / RADIX;
-					r = r * RADIX;
-					g = g / RADIX;
+					c /= RADIX;
+					r *= RADIX;
+					f /= RADIX;
 				}
-
-				// Convergence block.
-				if (c + r < 0.95 * s)
+				if (std::pow(c, p) + std::pow(r, p) < 0.95 * s)
 				{
-					converged = true;
-
-					// Apply similarity transformation to the rows and cols.
-					A.col(k).noalias() = A.col(k) * f;
-					A.row(k).noalias() = A.row(k) / f;
+					converged = false;
 
-					// ns_utils::print("f in the balancing algorithm : ", f);
+					A.col(k) *= f;
+					A.row(k) /= f;
 				}
-
 			}
-
 			else
 			{
-				// Apply similarity transformation to the rows and cols.
-				A.col(k).noalias() = A.col(k) * f;
-				A.row(k).noalias() = A.row(k) / f;
+				converged = false;
 			}
 		}
-
-	}
+	} while (!converged);
 
 }
 

control/autoware_control_toolbox/src/utils_act/state_space.cpp

@@ -179,6 +179,9 @@ void ns_control_toolbox::tf2ss::computeSystemMatrices(const std::vector<double>&
 
 	// Balance the matrices.
 	// Call balance method on the system matrices.
+	//	ns_utils::print("Unbalanced System Matrix.");
+	//	ns_eigen_utils::printEigenMat(sys_matABCD_cont_);
+
 	ns_control_toolbox::balance(sys_matABCD_cont_);
 }
 
@@ -285,52 +288,52 @@ void ns_control_toolbox::tf2ss::discretisize(double const& Ts)
 // Discrete time state-space matrices.
 Eigen::MatrixXd ns_control_toolbox::tf2ss::Ad() const
 {
-	auto&& Ad = sys_matABCD_disc_.topLeftCorner(N_ - 1, N_ - 1);
-	return Ad;
+//	auto&& Ad = sys_matABCD_disc_.topLeftCorner(N_ - 1, N_ - 1);
+	return sys_matABCD_disc_.topLeftCorner(N_ - 1, N_ - 1);
 }
 
 Eigen::MatrixXd ns_control_toolbox::tf2ss::Bd() const
 {
-	auto&& Bd = sys_matABCD_disc_.topRightCorner(N_ - 1, 1);
-	return Bd;
+//	auto&& Bd = sys_matABCD_disc_.topRightCorner(N_ - 1, 1);
+	return sys_matABCD_disc_.topRightCorner(N_ - 1, 1);
 }
 
 Eigen::MatrixXd ns_control_toolbox::tf2ss::Cd() const
 {
-	auto&& Cd = sys_matABCD_disc_.bottomLeftCorner(1, N_ - 1);
-	return Cd;
+//	auto&& Cd = sys_matABCD_disc_.bottomLeftCorner(1, N_ - 1);
+	return sys_matABCD_disc_.bottomLeftCorner(1, N_ - 1);
 }
 
 Eigen::MatrixXd ns_control_toolbox::tf2ss::Dd() const
 {
-	auto&& Dd = sys_matABCD_disc_.bottomRightCorner(1, 1);
-	return Dd;
+//	auto&& Dd = sys_matABCD_disc_.bottomRightCorner(1, 1);
+	return sys_matABCD_disc_.bottomRightCorner(1, 1);;
 }
 
 
 // Continuous time state-space matrices.
 Eigen::MatrixXd ns_control_toolbox::tf2ss::A() const
 {
-	auto&& A = sys_matABCD_cont_.topLeftCorner(N_ - 1, N_ - 1);
-	return A;
+//	auto A = sys_matABCD_cont_.topLeftCorner(N_ - 1, N_ - 1);
+	return sys_matABCD_cont_.topLeftCorner(N_ - 1, N_ - 1);
 }
 
 Eigen::MatrixXd ns_control_toolbox::tf2ss::B() const
 {
-	auto&& B = sys_matABCD_cont_.topRightCorner(N_ - 1, 1);
-	return B;
+//	auto&& B = sys_matABCD_cont_.topRightCorner(N_ - 1, 1);
+	return sys_matABCD_cont_.topRightCorner(N_ - 1, 1);
 }
 
 Eigen::MatrixXd ns_control_toolbox::tf2ss::C() const
 {
-	auto&& C = sys_matABCD_cont_.bottomLeftCorner(1, N_ - 1);
-	return C;
+//	auto&& C = sys_matABCD_cont_.bottomLeftCorner(1, N_ - 1);
+	return sys_matABCD_cont_.bottomLeftCorner(1, N_ - 1);
 }
 
 Eigen::MatrixXd ns_control_toolbox::tf2ss::D() const
 {
-	auto&& D = sys_matABCD_cont_.bottomRightCorner(1, 1);
-	return D;
+	// auto&& D = sys_matABCD_cont_.bottomRightCorner(1, 1);
+	return sys_matABCD_cont_.bottomRightCorner(1, 1);
 }
 
 

control/autoware_control_toolbox/usage_examples/main_balance.cpp

@@ -19,14 +19,9 @@ int main()
 	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());
@@ -39,9 +34,43 @@ int main()
 	ns_control_toolbox::balance(ss_system);
 	ns_eigen_utils::printEigenMat(ss_system);
 
+	// COMPARE with MATLAB RESULTS.
+	double tau_steer{ 0.3 };
+	double wheelbase{ 2.9 }; // L in vehicle model.
+
+	ns_control_toolbox::tf_factor m_den1{{ wheelbase, 0, 0 }}; // L*s^2
+	ns_control_toolbox::tf_factor m_den2{{ tau_steer, 1 }}; // (tau*s + 1)
+	auto den_tf_factor = m_den1 * m_den2;
+
+	ns_control_toolbox::tf Gey({ 1. }, den_tf_factor(), 4., 1.); // num, den, num constant, den constant
+	Gey.print();
+
+	// Qfilter
+	double dt = 1. / 40;
+	double frq = 20.;
+	double wc = 2 * M_PI * frq;
+	double tau_q = 1 / wc;
+
+	ns_control_toolbox::tf_factor den_q{{ tau_q, 1 }};
+	den_q.power(3);
+
+	auto Qtf = ns_control_toolbox::tf({ 1 }, { den_q() });
+	Qtf.print();
+
+	Gey.inv();
+	Gey.print();
+
+	auto QGinv = Qtf * Gey;
+	QGinv.print();
+
+	auto QGinvss = ns_control_toolbox::tf2ss(QGinv, dt);
+
+	ns_utils::print("Balanced System Matrices");
+	QGinvss.print();
 	// PERMUTATION TEST
 
 
 
+
 	return 0;
 }