本文整理汇总了C++中KalmanFilter::predictAndUpdate方法的典型用法代码示例。如果您正苦于以下问题：C++ KalmanFilter::predictAndUpdate方法的具体用法？C++ KalmanFilter::predictAndUpdate怎么用？C++ KalmanFilter::predictAndUpdate使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类KalmanFilter的用法示例。
在下文中一共展示了KalmanFilter::predictAndUpdate方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: executeCycle
int IMUTester::executeCycle(void)
{
bool newData = false;
this->readSerialCommand();
int sensorData;
finish = finish + 20;
stopTime = finish - (sumCollectionTime / cycles);
while (millis() <= stopTime)
{
while (Serial2.available())//&& millis() < finish)
{
char c = Serial2.read();
//serialPrint(c);
if (gps.encode(c))
newData = true;
}
}
sensorCollectionStart = millis();
//serialPrint("Accelerometer: \n");
//gyro.readRawX();
//serialPrint(accel.getX()); serialPrint("\n");
int accX = accel.getX(); // serialPrint(",");
int accY = accel.getY(); // serialPrint(",");
int accZ = accel.getZ(); // serialPrint(",");
//Serial1.print("\n");
//serialPrint("Gyroscope: \n");
int gyroX = gyro.getRotationX(); // serialPrint(",");
int gyroY = gyro.getRotationY(); // serialPrint(",");
int gyroZ = gyro.getRotationZ(); // serialPrint(",");
//serialPrint("Compass: \n");
int compX = comp.getRawX(); // serialPrint(",");
int compY = comp.getRawY(); // serialPrint(",");
int compZ = comp.getRawZ(); // serialPrint(",");
gps.get_datetime(&date, &time);
if (newData && time != oldTime)
{
gps.get_position(&newLong, &newLat); //, &fixAge);
// serialPrint(newLat); serialPrint(",");
// serialPrint(newLong); serialPrint(",");
// serialPrint(gps.altitude()); serialPrintln(",1");
oldTime = time;
}
else
{
// serialPrintln("0,0,0,0");
}
sumCollectionTime += (millis() - sensorCollectionStart);
cycles++;
serialPrint("\n");
kalman.assignSensorValues(
accX, accY, accZ,
gyroX, gyroY, gyroZ,
compX, compY, compZ,
0,0,0,cycles % 5 == 0);
kalman.predictAndUpdate();
quadState_t covar = kalman.getCovariance();
quadState_t state = kalman.getQuadState();
serialPrint("Xa ");serialPrint(state.xAngle); serialPrint("\t");
serialPrint("Ya ");serialPrint(state.yAngle); serialPrint("\t");
serialPrint("Za ");serialPrint(state.zAngle);
// serialPrint("Xr ");serialPrint(state.xRotation); serialPrint("\t");
// serialPrint("Yr ");serialPrint(state.yRotation); serialPrint("\t");
// // serialPrint("Zr ");serialPrint(state.zRotation);
return 0;
}