本文整理汇总了C++中Vector4d::rows方法的典型用法代码示例。如果您正苦于以下问题:C++ Vector4d::rows方法的具体用法?C++ Vector4d::rows怎么用?C++ Vector4d::rows使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector4d
的用法示例。
在下文中一共展示了Vector4d::rows方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: measurementUpdateDecaWave
// 6. Total measurement update
void Ekf::measurementUpdateDecaWave(Vector4d z){
// Determine h and H
Vector4d h;
h = hDecaWave(state_, DecaWaveBeaconLoc_, DecaWaveOffset_);
Matrix46 H;
H = HDecaWave(state_, DecaWaveBeaconLoc_, DecaWaveOffset_);
// Find Kalman Gain
// First check if any z values are -1 (i.e. a bad measurement). If so, make
// the corresponding entry in RDecaWave_ very high for this iteration. This
// effectively cancels out any influence by that measurement on the update.
Matrix4d RDecaWaveTemp;
for (int i = 0; i < z.rows(); i++) {
if (z(i) < 0) {
RDecaWaveTemp(i,i) = 1e20;
}
else {
RDecaWaveTemp(i,i) = RDecaWave_(i,i);
}
}
// Now go on to calculate the Kalman gain with the new R value.
Matrix64 K;
K = KDecaWave(cov_, H, RDecaWaveTemp);
// Find new state
state_ = stateUpdateDecaWave(state_, K, z, h);
// Find new covariance
cov_ = covUpdateDecaWave(cov_, K, H);
cov_ = zeroOutBiasXYThetaCov(cov_);
}