本文整理汇总了C++中Vector4f::cwiseAbs2方法的典型用法代码示例。如果您正苦于以下问题：C++ Vector4f::cwiseAbs2方法的具体用法？C++ Vector4f::cwiseAbs2怎么用？C++ Vector4f::cwiseAbs2使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector4f的用法示例。
在下文中一共展示了Vector4f::cwiseAbs2方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: update
void LIPStateEstimator::update(float timePassed, const Array2f& LIPHeights, const Vector2f& leftOrigin, const SupportFoot newSupportFoot)
{
this->LIPHeights = LIPHeights;
Vector2f newOrigin = leftOrigin;
if(newSupportFoot != SupportFoot::left)
newOrigin.y() *= -1;
const Vector4f measurement = measure(newSupportFoot, newOrigin);
if(supportFoot != newSupportFoot)
{
EstimatedState est = getEstimate();
est = convertToOtherFoot(est);
ukf.mean.head<2>() = est.com.position;
ukf.mean.segment<2>(2) = est.com.velocity;
ukf.mean.tail<2>() = est.zmp;
}
else
{
ukf.mean.head<2>() = origin + ukf.mean.head<2>() - newOrigin;
ukf.mean.tail<2>() = origin + ukf.mean.tail<2>() - newOrigin;
}
supportFoot = newSupportFoot;
origin = newOrigin;
auto dynamicMoldel = [&](Vector6f& state)
{
const Vector2f zmp = state.tail<2>();
LIP3D lipState(state.head<2>(), state.segment<2>(2), LIPHeights);
lipState.update(timePassed, zmp);
state << lipState.position, lipState.velocity, zmp;
};
auto measurementModel = [&](const Vector6f& state)
{
return (Vector4f() << state.head<2>(), state.tail<2>()).finished();
};
const Vector6f dynamicNoise = (Vector6f() << params.positionProcessDeviation, params.velocityProcessDeviation,
params.zmpProcessDeviation).finished() * timePassed;
const Vector4f measurementNoise = (Vector4f() << params.positionMeasurementDeviation, params.zmpMeasurementDeviation).finished() * timePassed;
ukf.predict(dynamicMoldel, dynamicNoise.cwiseAbs2().asDiagonal());
ukf.update<4>(measurement, measurementModel, measurementNoise.cwiseAbs2().asDiagonal());
}