本文整理汇总了C++中KalmanFilter::initialize方法的典型用法代码示例。如果您正苦于以下问题：C++ KalmanFilter::initialize方法的具体用法？C++ KalmanFilter::initialize怎么用？C++ KalmanFilter::initialize使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类KalmanFilter的用法示例。
在下文中一共展示了KalmanFilter::initialize方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: KalmanFilter
TEST (KalmanFilterTest, KalmanFilterCanInitialize) {
KalmanFilter *kf = new KalmanFilter();
kf->initialize();
ASSERT_EQ(kf->getRelXPosEst(), 0.f);
ASSERT_EQ(kf->getRelYPosEst(), 0.f);
ASSERT_EQ(kf->getRelXVelEst(), 1.f);
ASSERT_EQ(kf->getRelYVelEst(), 0.5f);
ufvector4 initX = boost::numeric::ublas::zero_vector<float> (4);
initX(0) = 10.f;
initX(1) = 11.f;
initX(2) = 12.f;
initX(3) = 13.f;
ufmatrix4 initCov = boost::numeric::ublas::identity_matrix<float> (4);
initCov(0,0) = 1.f;
initCov(1,1) = 2.f;
initCov(2,2) = 3.f;
initCov(3,3) = 4.f;
kf->initialize(initX, initCov);
ASSERT_EQ(kf->getRelXPosEst(), 10.f);
ASSERT_EQ(kf->getRelYPosEst(), 11.f);
ASSERT_EQ(kf->getRelXVelEst(), 12.f);
ASSERT_EQ(kf->getRelYVelEst(), 13.f);
ASSERT_EQ(kf->getCovXPosEst(), 1.f);
ASSERT_EQ(kf->getCovYPosEst(), 2.f);
ASSERT_EQ(kf->getCovXVelEst(), 3.f);
ASSERT_EQ(kf->getCovYVelEst(), 4.f);
}示例2: initialize
/**
* @brief - Initialize all the filters!
* @params- given a relX and relY for the position mean
* also a covX and covY since we may want to init
* w/ diff certainties throughout the life
* @choice I chose to have the velocities randomly initialized since there are
soooo many combos
*/
void MMKalmanFilter::initialize(float relX, float relY, float covX, float covY)
{
// clear the filters
filters.clear();
// make a random generator for initilizing different filters
boost::mt19937 rng;
rng.seed(std::time(0));
boost::uniform_real<float> posCovRange(-2.f, 2.f);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > positionGen(rng, posCovRange);
boost::uniform_real<float> randVelRange(-5.f, 5.f);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > velocityGen(rng, randVelRange);
//make stationary
for (int i=0; i<params.numFilters/2; i++)
{
// Needs to be stationary, have given mean, and add noise
// to the covariance matrix
KalmanFilter *stationaryFilter = new KalmanFilter(true);
ufvector4 x = boost::numeric::ublas::zero_vector<float>(4);
x(0) = relX;
x(1) = relY;
x(2) = 0.f;
x(3) = 0.f;
ufmatrix4 cov = boost::numeric::ublas::zero_matrix<float>(4);
cov(0,0) = covX + positionGen();
cov(1,1) = covY + positionGen();
// init and push it back
stationaryFilter->initialize(x, cov);
filters.push_back(stationaryFilter);
}
// make moving
for (int i=0; i<params.numFilters/2; i++)
{
// Needs to be stationary, have given mean, and add noise
// to the covariance matrix
KalmanFilter *movingFilter = new KalmanFilter(false);
ufvector4 x = boost::numeric::ublas::zero_vector<float>(4);
x(0)= relX;
x(1)= relY;
x(2) = 0.f;
x(3) = 0.f;
// Choose to assum obsv mean is perfect and just have noisy velocity
ufmatrix4 cov = boost::numeric::ublas::zero_matrix<float>(4);
cov(0,0) = covX;
cov(1,1) = covY;
cov(2,2) = 20.f;
cov(3,3) = 20.f;
movingFilter->initialize(x, cov);
filters.push_back(movingFilter);
}
}示例3: setupTester
int IMUTester::setupTester(void)
{
#ifdef SERIAL_USB
Serial.begin(115200);
sensorMode = ALL;
#endif
#ifdef SERIAL_BLUETOOTH
Serial1.begin(115200, SERIAL_8N1); // baud rate = 115200, 8 data bits, no parity, 1 stop bit
Serial1.println("R,1"); // restart it
#endif
kalman.initialize( 6.9158f, 9.9410f, 21.515f,
-16.027f, 0.9157f, 0.6185f,
// -12504.f, -13316.f, -24942.f );
0,0,0);
i2c.initialize();
serialPrint("IMUTester::setupTester called");
//acc.setup();
accel.begin();
gyro.initialize();
//dataRate_t maxDataRate = ADXL345_DATARATE_3200_HZ;
//accel.setDataRate(maxDataRate);
//gyro.setRate(2);
delay(1000);
//bar.setup();
comp.setup();
//gpssetup {
Serial2.begin(57600);
delay(1000);
//}
// Serial1.begin(115200, SERIAL_8N1); // baud rate = 115200, 8 data bits, no parity, 1 stop bit
// Serial1.println("R,1");
cycles = 1;
sumCollectionTime = 5;
//serialPrint("a.x,a.y,a.z,g.x,g.y,g.z,c.x,c.y,G.x,G.y,G.z,G.a");
finish = millis() + 100;
return 0;
}