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);
}
/**
* @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);
}
}
TEST (KalmanFilterTest, Works) {
KalmanFilter *kf = new KalmanFilter(true);
ufvector4 initX = boost::numeric::ublas::zero_vector<float> (4);
initX(0) = 50.f;
initX(1) = 0.f;
initX(2) = 0.f;
initX(3) = 0.f;
ufmatrix4 initCov = boost::numeric::ublas::identity_matrix<float> (4);
initCov(0,0) = 5.f;
initCov(1,1) = 5.f;
initCov(2,2) = 0.f;
initCov(3,3) = 0.f;
kf->initialize(initX, initCov);
kf->predict(kf->genOdometry(-5.f, 0.f, 0.f),10.f);
kf->updateWithObservation(kf->genVisBall(58.f, 0.f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(55.f, .05f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(55.f, .05f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(55.f, .05f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(83.f, .05f));
}
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;
}
TEST (KalmanFilterTest, FilterCanPredict) {
KalmanFilter *kf = new KalmanFilter(false);
kf->initialize();
float initX = 0.f;
float initY = 0.f;
float initXVel = 1.f;
float initYVel = 0.5f;
float finalX = 4.f;
float finalY = 2.f;
ASSERT_EQ(initX, kf->getRelXPosEst());
ASSERT_EQ(initY, kf->getRelYPosEst());
messages::RobotLocation odometry;
odometry.set_x(0.f);
odometry.set_y(0.f);
odometry.set_h(0.f);
kf->predict(odometry, 4.f);
ASSERT_EQ(finalX, kf->getRelXPosEst());
ASSERT_EQ(finalY, kf->getRelYPosEst());
}
