Esempio n. 1
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. 2
0
TEST (KalmanFilterTest, MMWorks) {
    MMKalmanFilter *kf = new MMKalmanFilter();
    KalmanFilter *poor = new KalmanFilter();

    kf->initialize(50.f, 0.f, 5.f, 5.f);

    messages::Motion inpMotion;
    inpMotion.mutable_odometry()->CopyFrom(poor->genOdometry(0.f,0.f,0.f));

    kf->update(poor->genVisBall(55.5f, 0.f),
               inpMotion);

    kf->update(poor->genVisBall(62.f, 0.f),
               inpMotion);

    kf->update(poor->genVisBall(64.f, 0.f),
               inpMotion);

    kf->update(poor->genVisBall(71.f, 0.f),
               inpMotion);
    kf->printEst();
/*
    kf->update(poor->genVisBall(75.f, 0.f),
               inpMotion);


    kf->update(poor->genVisBall(74.f, 0.f),
               inpMotion);

    kf->update(poor->genVisBall(76.f, 0.f),
               inpMotion);

    kf->update(poor->genVisBall(75.f, 0.f),
               inpMotion);
    kf->update(poor->genVisBall(75.f, 0.f),
               inpMotion);
    kf->update(poor->genVisBall(75.f, 0.f),
               inpMotion);
    kf->update(poor->genVisBall(75.f, 0.f),
               inpMotion);
    kf->update(poor->genVisBall(75.f, 0.f),
               inpMotion);

 */




}