本文整理汇总了C++中KalmanFilter::getUMatrix方法的典型用法代码示例。如果您正苦于以下问题：C++ KalmanFilter::getUMatrix方法的具体用法？C++ KalmanFilter::getUMatrix怎么用？C++ KalmanFilter::getUMatrix使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类KalmanFilter的用法示例。
在下文中一共展示了KalmanFilter::getUMatrix方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
//.........这里部分代码省略.........
float accelZMean = accelZ - accelMeans[2];
float gyroX = gyroData[i][0] - gyroMeans[0];
float gyroY = gyroData[i][1] - gyroMeans[1];
float gyroZ = gyroData[i][2] - gyroMeans[2];
float magX = magData[i][0];
float magY = magData[i][1];
float magZ = magData[i][2];
float pX = 0.0f;
float pY = 0.0f;
float pZ = 0.0f;
float vX = lastMeasurement[3] + (accelXMean * dt);
float vY = lastMeasurement[4] + (accelYMean * dt);
float vZ = lastMeasurement[5] + (accelZMean * dt);
float accelAngleZ = accelZ + (1.0f - accelMeans[2]);
// Calculate attitude_x from y and z component of gravity vector
float aX = atan2(accelY, accelAngleZ) * (180.0f / 3.14f);
// Calculate attitude_y from x and z component of gravity vector
float aY = atan2(accelX, accelAngleZ) * (180.0f / 3.14f);
// Calculate attitude_z from x and z component of mag reading
// (probably will not be accurate)
float aZ = atan2(magX, magZ) * (180.0f / 3.14f);
accelAnglesFile << aX << ", " << aY << ", " << aZ << endl;
float measurement[12] =
{
aX,
aY,
aZ,
pX,
pY,
pZ,
vX,
vY,
vZ,
0.0f,
0.0f,
0.0f
};
float measurement6[6] =
{
aX,
aY,
aZ,
pX,
pY,
pZ
};
kalmanFilter.getUMatrix() << gyroX, gyroY, gyroZ;
kalmanFilter6.getUMatrix() << gyroX, gyroY, gyroZ;
// Predict
kalmanFilter.predict();
kalmanFilter6.predict();
kalmanFilter.update(measurement);
kalmanFilter6.update(measurement6);
filteredStatesFile <<
kalmanFilter.getXMatrix()(0, 0) << ", " <<
kalmanFilter.getXMatrix()(1, 0) << ", " <<
kalmanFilter.getXMatrix()(2, 0) << ", " <<
kalmanFilter.getXMatrix()(3, 0) << ", " <<
kalmanFilter.getXMatrix()(4, 0) << ", " <<
kalmanFilter.getXMatrix()(5, 0) << ", " <<
kalmanFilter.getXMatrix()(6, 0) << ", " <<
kalmanFilter.getXMatrix()(7, 0) << ", " <<
kalmanFilter.getXMatrix()(8, 0) << ", " <<
kalmanFilter.getXMatrix()(9, 0) << ", " <<
kalmanFilter.getXMatrix()(10, 0) << ", " <<
kalmanFilter.getXMatrix()(11, 0);
filteredStatesFile << endl;
filteredStatesFile6 <<
kalmanFilter6.getXMatrix()(0, 0) << ", " <<
kalmanFilter6.getXMatrix()(1, 0) << ", " <<
kalmanFilter6.getXMatrix()(2, 0) << ", " <<
kalmanFilter6.getXMatrix()(3, 0) << ", " <<
kalmanFilter6.getXMatrix()(4, 0) << ", " <<
kalmanFilter6.getXMatrix()(5, 0);
filteredStatesFile6 << endl;
memcpy(measurement, lastMeasurement, 12 * 4);
memcpy(measurement6, lastMeasurement6, 6 * 4);
}
filteredStatesFile.close();
filteredStatesFile6.close();
accelAnglesFile.close();
return 0;
}