本文整理汇总了C++中VectorXd::middleRows方法的典型用法代码示例。如果您正苦于以下问题:C++ VectorXd::middleRows方法的具体用法?C++ VectorXd::middleRows怎么用?C++ VectorXd::middleRows使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类VectorXd
的用法示例。
在下文中一共展示了VectorXd::middleRows方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: operator
VectorXd LinearizedControlError::operator()(const VectorXd& a) const {
Vector6d x1 = a.topRows(6);
Vector6d x2 = a.middleRows(6,6);
double phi = a(12), Delta = a(13), curvature = a(14);
double phi_ref = cfg0->phi, Delta_ref = cfg0->Delta, curvature_ref = cfg0->curvature;
MatrixXd F = MatrixXd::Zero(6,6);
F.topLeftCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
F.topRightCorner(3, 3) = -rotMat(Vector3d(0, 0, Delta_ref));
F.bottomRightCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
MatrixXd G = MatrixXd::Zero(6,6);
G(2, 0) = 1;
G(3, 2) = 1;
G(5, 1) = 1;
MatrixXd A = F.exp();
MatrixXd halfF = 0.5*F;
MatrixXd B = (1/6.0) * (G + 4.0*(halfF.exp()*G) + A*G);
Vector3d u;
u << Delta - Delta_ref, phi - phi_ref, Delta*curvature - Delta_ref*curvature_ref;
return x2 - A * x1 - B * u;
}