Esempio n. 1
0
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);
}
Esempio n. 2
0
/**
 * @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);
    }
}
Esempio n. 3
0
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));
}
Esempio n. 4
0
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;
}
Esempio n. 5
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());
}