예제 #1
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);
    }
}
예제 #2
0
int main(int /*argc*/, char * /*argv*/[]) {
	std::srand(std::time(NULL));

	/// We want a simple 2d state
	typedef SimpleState<2> state_t;

	/// Our process model is the simplest possible: doesn't change the mean
	typedef ConstantProcess<2, state_t> process_t;

	/// Create a kalman filter instance with our chosen state and process types
	KalmanFilter<state_t, process_t> kf;

	/// Set our process model's variance
	kf.processModel.sigma = state_t::VecState::Constant(6.5);

	double dt = 0.5;
	double sumSquaredError = 0;
	
	const double measurementVariance = 9; //NOISE_AMPLITUDE / 2.0;

	/// for sine curve
	
	std::vector<StatePair> data = generateSineData(dt);
	/// CSV header row
	std::cout << "actual x,actual y,measurement x,measurement y,filtered x,filtered y,squared error" << std::endl;
	double t = 0;
	for (unsigned int i = 0; i < data.size(); ++i) {
		/// Predict step: Update Kalman filter by predicting ahead by dt
		kf.predict(dt);

		/// "take a measurement" - in this case, noisify the actual measurement
		Eigen::Vector2d pos = data[i].first;
		Eigen::Vector2d m = data[i].second;
		AbsoluteMeasurement<state_t> meas;
		meas.measurement = m;
		meas.covariance = Eigen::Vector2d::Constant(measurementVariance).asDiagonal();

		/// Correct step: incorporate information from measurement into KF's state
		kf.correct(meas);

		/// Output info for csv
		double squaredError = (pos[0] - kf.state.x[0]) * (pos[0] - kf.state.x[0]);
		sumSquaredError += squaredError;
		std::cout << pos[0] << "," << pos[1] << ",";
		std::cout << meas.measurement[0] << "," << meas.measurement[1] << ",";
		std::cout << kf.state.x[0] << "," << kf.state.x[1] << ",";
		std::cout << squaredError;
		std::cout << std::endl;
		
		t += dt;

	}
	std::cerr << "Sum squared error: " << sumSquaredError << std::endl;
	return 0;

}
예제 #3
0
KalmanFilter* TargetTracker::findFilter(int grpId, int targetId)
{
    KalmanFilter *filter = NULL;
    filter = filters[grpId * 100000 + targetId];
    if (filter == NULL) {
        filter = new KalmanFilter();    // 使用默认的运动模型
        filter->setGroupId(grpId);
        filter->setTargetId(targetId);
        filters[grpId * 100000 + targetId] = filter;
    }
    return filter;
}
예제 #4
0
void ocvKalmanApp::mouseDrag( MouseEvent event )
{
    mMousePoints.push_back( event.getPos() );
    
    if( mKalmanPoints.empty() ) {
        mFilter = new KalmanFilter( toOcv( event.getPos() ) );
        mKalmanPoints.push_back( event.getPos() );
    } else {
        mFilter->correct( toOcv( event.getPos() ) );
        mKalmanPoints.push_back( fromOcv( mFilter->updatePrediction() ) );
        console() << mKalmanPoints.back() << endl;
    }
}
예제 #5
0
TEST (KalmanFilterTest, FilterCanUpdate) {
    KalmanFilter *kf = new KalmanFilter(true);
    kf->isUpdated();
    bool yes = true;
    bool no = false;
    ASSERT_EQ(no, kf->isUpdated());
    kf->setUpdate(true);
    ASSERT_EQ(yes, kf->isUpdated());

    int a =1;
    int b =1;
    ASSERT_EQ(a,b);
}
int main (int argc, char **argv){
  int const		accSigma    = 1;
  int const             t = (argc == 2)?(lexical_cast<int>(argv[1])):(4);
  double const		deltaT	    = 1.0;
  dmat			A(2,2), B(2,2), G(2,1), Q(2,2);
  dmat			State(2,1), Cov(2,2), Ctrl(2,1);
  std::vector<dmat>	Result;

  A(0,0) = 1;A(0,1) = deltaT;
  A(1,0) = 0;A(1,1) = 1;

  B(0,0) = 0;B(0,1) = 0;
  B(1,0) = 0;B(1,1) = 0;

  G(0,0) = deltaT * deltaT * 0.5;
  G(1,0) = deltaT;
  Q = accSigma * prod(G, trans(G));
  
  State(0,0) = 0;
  State(1,0) = 0;

  Cov(0,0) = 1;Cov(0,1) = 0;
  Cov(1,0) = 0;Cov(1,1) = 1;

  Ctrl(0,0) = 0;
  Ctrl(1,0) = 0;

  KalmanFilter kf = KalmanFilter(&A, &B, &Q, accSigma);

  for(int i = 0; i < t; ++i){
    string fileName = "KFCorrect(k=" + lexical_cast<string>(i + 1) + ").txt";
    ofstream ofs(fileName.c_str());
    Result = kf.Correct(&State, &Cov, &Ctrl, &G);
    State = Result.at(0);
    Cov = Result.at(1);

    for(double location = -20.0; location <= 20.0; location += 0.5){
      for(double vel = -10.0; vel <= 10.0; vel += 0.5){
	dmat Info(2,1);
	Info(0,0) = location;
	Info(1,0) = vel;
	double prob = kf.calcBelief(&Info, &State, &Cov);
	
	ofs << location << "\t" << vel << "\t" << prob << endl;
      }
    }
  }

  return 0;
}
예제 #7
0
KalmanFilter execute_time_step(KalmanFilter KF,
			      Mat TransitionModel,
			      Mat MeasurementModel,
			      Mat ControlModel,
			      Mat measurement,
			      Mat control)
{
  KF.transitionMatrix = TransitionModel;
  KF.measurementMatrix = MeasurementModel;
  KF.controlMatrix = ControlModel;
  KF.predict(control);
  KF.correct(measurement);
  return KF;  
}
예제 #8
0
int main(int argc, char **argv)
{
  ros::init(argc, argv, "pose_filter");
  ros::NodeHandle nh;

  tf::TransformListener lr;
  tf::StampedTransform laser_trans, kinect_trans;

  geometry_msgs::PoseStamped target_pose;
  ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("leader_pose", 1);
  target_pose.pose.position.z = 0; target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
  target_pose.header.frame_id = "map";

  Mat measurement = Mat(2, 1, CV_32FC1);
  KalmanFilter kalman = KalmanFilter(4,2,0);
  setup_kalman(kalman);

  ros::Rate kalman_rate(KALMAN_HZ);
  while(ros::ok())
  {
	try
	{
		lr.lookupTransform("map", "laser_target", ros::Time(0), laser_trans);
	}
	catch(tf::TransformException ex) {}
	/*try
	{
		lr.lookupTransform("odom", "kinect_target", ros::Time(0), kinect_trans);
	}
	catch(tf::TransformException ex) {}

	measurement.at<float>(0,0) = (laser_trans.getOrigin().x() + kinect_trans.getOrigin().x())/2;
	measurement.at<float>(1,0) = (laser_trans.getOrigin().y() + kinect_trans.getOrigin().y())/2;*/
	measurement.at<float>(0,0) = laser_trans.getOrigin().x();
	measurement.at<float>(1,0) = laser_trans.getOrigin().y();

	Mat predict = kalman.predict();	kalman.correct(measurement);

	target_pose.pose.position.x = kalman.statePost.at<float>(0,0);
	target_pose.pose.position.y = kalman.statePost.at<float>(1,0);
	target_pose.header.stamp = laser_trans.stamp_;

	pub.publish(target_pose);
	kalman_rate.sleep();
  }

  return 0;
}
예제 #9
0
void initKalman(float x, float y)
{
    // Instantate Kalman Filter with
    // 4 dynamic parameters and 2 measurement parameters,
    // where my measurement is: 2D location of object,
    // and dynamic is: 2D location and 2D velocity.
    KF.init(4, 2, 0);

    measurement = Mat_<float>::zeros(2,1);
    measurement.at<float>(0, 0) = x;
    measurement.at<float>(0, 0) = y;


    KF.statePre.setTo(0);
    KF.statePre.at<float>(0, 0) = x;
    KF.statePre.at<float>(1, 0) = y;

    KF.statePost.setTo(0);
    KF.statePost.at<float>(0, 0) = x;
    KF.statePost.at<float>(1, 0) = y; 

    KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(.1));
}
예제 #10
0
// startTracking()
//
void startTracking(IplImage * pImg, CvRect pHandRect,KalmanFilter &kfilter)
{
	float maxVal = 0.f;

	// Make sure internal data structures have been allocated
	if( !pHist ) createTracker(pImg);

	// Create a new hue image
	updateHueImage(pImg);

    if(!((pHandRect.x<0)||(pHandRect.y<0)||((pHandRect.x+pHandRect.width)>pImg->width)||((pHandRect.y+pHandRect.height)>pImg->height))) {

	// Create a histogram representation for the hand
    cvSetImageROI( pHueImg, pHandRect );
    cvSetImageROI( pMask,   pHandRect );
    cvCalcHist( &pHueImg, pHist, 0, pMask );
    cvGetMinMaxHistValue( pHist, 0, &maxVal, 0, 0 );
    cvConvertScale( pHist->bins, pHist->bins, maxVal? 255.0/maxVal : 0, 0 );
    cvResetImageROI( pHueImg );
    cvResetImageROI( pMask );

	}
	// Store the previous hand location
	prevHandRect =pHandRect;
	prevHandRect2 =pHandRect;

	//Pass the hand location to kalman initializer
	kfilter.predictionBegin(prevHandRect);
	
}
int Kalman_adjust(Point A,int biggest,KalmanFilter &F)
{
	Mat_<float> measurement(2,1);
	if(!notstarted[biggest])
	{
		notstarted[biggest]=true;
		 measurement.setTo(Scalar(0));
		F.statePre.at<float>(0) = A.x;
		F.statePre.at<float>(1) = A.y;
		F.statePre.at<float>(2) = 0;
		F.statePre.at<float>(3) = 0;
		F.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,0,0,   0,1,0,0,  0,0,1,0,  0,0,0,1);
		setIdentity(F.measurementMatrix);
		setIdentity(F.processNoiseCov, Scalar::all(1e-4));
		setIdentity(F.measurementNoiseCov, Scalar::all(1e-1));
		setIdentity(F.errorCovPost, Scalar::all(.1));
	}
	else
	{
		measurement(0) = A.x;
		measurement(1) = A.y;
		Mat estimated = F.correct(measurement);
		Point statePt(estimated.at<float>(0),estimated.at<float>(1));
	}
}
예제 #12
0
CvRect combi_track(IplImage * pImg,KalmanFilter &kfilter)
{
	CvRect predrect=kfilter.predictionReport(prevHandRect);	
	//if((predrect.x<0)||(predrect.y<0)||((predrect.x+predrect.width)>pImg->width)||((predrect.y+predrect.height)>pImg->height))
	//	return NULL;	
	CvConnectedComp components;

	// Create a new hue image
	updateHueImage(pImg);

	// Create a probability image based on the hand histogram
	cvCalcBackProject( &pHueImg, pProbImg, pHist );
    cvAnd( pProbImg, pMask, pProbImg, 0 );
	//cvSetImageROI(pProbImg,predrect);
	
	// Use CamShift to find the center of the new hand probability
    if(!((predrect.x<0)||(predrect.y<0)||((predrect.x+predrect.width)>pImg->width)||((predrect.y+predrect.height)>pImg->height))) {
        cvCamShift( pProbImg, predrect, cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),&components, handBox );
	// Update hand location and angle
    prevHandRect = components.rect;

    }
    else
        //cvCamShift( pProbImg, prevHandRect, cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),&components, handBox );
		prevHandRect.x=-1;
	
    //if(!pImg->origin)	
	//	handBox->angle = -handBox->angle;
	//cvResetImageROI(pProbImg);
	
	return prevHandRect;

}
예제 #13
0
//-----------------------------------------------------------------------------
//
void setup()
{
    Serial.begin(9600);
    Wire.begin();
    i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
    i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
    i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
    i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
    while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
    while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode

    while(i2cRead(0x75,i2cData,1));
    if(i2cData[0] != 0x68)   // Read "WHO_AM_I" register
    {
        Serial.print(F("Error reading sensor"));
        while(1);
    }

    delay(100); // Wait for sensor to stabilize

    /* Set kalman and gyro starting angle */
    while(i2cRead(0x3B,i2cData,6));
    accX = ((i2cData[0] << 8) | i2cData[1]);
    accY = ((i2cData[2] << 8) | i2cData[3]);
    accZ = ((i2cData[4] << 8) | i2cData[5]);
    // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
    // We then convert it to 0 to 2π and then from radians to degrees
    accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
    accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;

    kalmanX.setAngle(accXangle); // Set starting angle
    kalmanY.setAngle(accYangle);
    gyroXangle = accXangle;
    gyroYangle = accYangle;
    compAngleX = accXangle;
    compAngleY = accYangle;

    TCCR2B = TCCR2B & 0b11111000 | 0x05;

    m1.Initialize();
    m2.Initialize();

    m1.Activate();
    m2.Activate();

    timer = micros();
}
예제 #14
0
cv::Point getKalmanCentroid(int x, int y)
{
  cv::Point kalman_centroid;
  mouse_info.x = x;
  mouse_info.y = y;
  
  Mat prediction = KF.predict();
  Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
  
  measurement(0) = mouse_info.x;
  measurement(1) = mouse_info.y;
  
  Point measPt(measurement(0),measurement(1));
  mousev.push_back(measPt);
  // generate measurement
  //measurement += KF.measurementMatrix*state;
  
  Mat estimated = KF.correct(measurement);
  Point statePt(estimated.at<float>(0),estimated.at<float>(1));
  kalmanv.push_back(statePt);
  
  //drawCross( statePt, Scalar(0,255,0), 5 ); // Kalman
  //drawCross( measPt, Scalar(0,0,255), 5 ); // Original
    
  // for (int i = 0; i < mousev.size()-1; i++) {
  //   line(kalman_img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
  // }
  // for (int i = 0; i < kalmanv.size()-1; i++) {
  //   line(kalman_img, kalmanv[i], kalmanv[i+1], Scalar(0,255,0), 1);
  // }
  
  // if (contador > 30)
  //   {
  //     initializeKalman();
  //   }
  // else
  //   {
  //     //ROS_INFO(">>> MEDIDA(%d,%d) KALMAN(%d,%d)", measPt.x, measPt.y, statePt.x, statePt.y);
  //   }
  //imshow("mi img", img);

  //imshow("Kalman", kalman_img);
  kalman_centroid.x = statePt.x;
  kalman_centroid.y = statePt.y; 
  return kalman_centroid;
}
예제 #15
0
Point kalmanCorrect(float x, float y)
{
    measurement(0) = x;
    measurement(1) = y;
    Mat estimated = KF.correct(measurement);
    Point statePt(estimated.at<float>(0),estimated.at<float>(1));
    return statePt;
}
예제 #16
0
파일: widget.cpp 프로젝트: JuarezASF/Code
vector<Point> Widget::findKalmanCenters(vector<Point> dataInput){
    vector<Point> kalmanCenters;
    if(dataInput.empty()){
        errorMsg("Nao ha objetos a serem detectados!");
        return kalmanCenters;
    }
    kalmanCenters.resize(dataInput.size());
    KalmanFilter *KF;

    for(unsigned int i = 0; i < targets.size(); i++){
        KF = targets[i]->KF;

        //apenas conversao de estruturas usadas
        Mat_<float> measurement(2,1);
        measurement(0) = dataInput[i].x;
        measurement(1) = dataInput[i].y;

        Mat estimated;
        Mat prediction = KF->predict();
        if(detectionSucess[i] == true)
            {
            //caso a detecção tenha sido um sucesso jogamos a nova medida no filtro
            //ao chamar correct já estamos adicionando a nova medida ao filtro
            estimated = KF->correct(measurement);
            }
        else{
            /*caso a medição tenha falhada realimentamos o filtro com a própria
            predição anterior*/
            Mat_<float> predictedMeasurement(2,1);
            predictedMeasurement(0) = prediction.at<float>(0);
            predictedMeasurement(1) = prediction.at<float>(1);
            estimated = KF->correct(predictedMeasurement);
            KF->errorCovPre.copyTo(KF->errorCovPost);
            //copiar a covPre para covPos [dica de um usuário de algum fórum]
            }
        Point statePt(estimated.at<float>(0),estimated.at<float>(1));

        kalmanCenters[i] = statePt;
        /*existe o centro medido pela previsão e o centro que o filtro de kalman
        acredita ser o real. O centro de kalman é uma ponderação das medidas e do modelo
        com conhecimento prévio de um erro aleatório*/
    }

    return kalmanCenters;
}
int Kalman_Perdict(KalmanFilter &F)
{
	if(notstarted[0] and notstarted[1] and notstarted[2])
	{
		Mat prediction = F.predict();
		Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
		MyFilledCircle(skin2,predictPt,1);
	}
	return 0;
}
예제 #18
0
파일: fc.cpp 프로젝트: DD1984/skydrop_stm32
void fc_manual_alt0_change(float val)
{
    kalmanFilter.reset(val);

    if (fc.flight.state == FLIGHT_WAIT || fc.flight.state == FLIGHT_LAND)
    {
        fc.flight.autostart_altitude = val;
        fc.altitude1 = val;
    }
}
예제 #19
0
JNIEXPORT jlong JNICALL Java_org_opencv_video_KalmanFilter_predict_11
  (JNIEnv* env, jclass , jlong self)
{
    try {
        LOGD("video::predict_11()");
        KalmanFilter* me = (KalmanFilter*) self; //TODO: check for NULL
        Mat _retval_ = me->predict(  );
        return (jlong) new Mat(_retval_);
    } catch(cv::Exception e) {
        LOGD("video::predict_11() catched cv::Exception: %s", e.what());
        jclass je = env->FindClass("org/opencv/core/CvException");
        if(!je) je = env->FindClass("java/lang/Exception");
        env->ThrowNew(je, e.what());
        return 0;
    } catch (...) {
        LOGD("video::predict_11() catched unknown exception (...)");
        jclass je = env->FindClass("java/lang/Exception");
        env->ThrowNew(je, "Unknown exception in JNI code {video::predict_11()}");
        return 0;
    }
}
예제 #20
0
void TargetTracker::tracking(std::vector<TargetState> states)
{
    std::vector<TargetState>::const_iterator iter;
    TargetState tState;
    KalmanFilter *filter;
    SingleTarget *target;
    qDebug() << "测量值:";

    for (iter = states.begin(); iter != states.end(); ++iter)
    {
        tState = *iter;
        tState.state.print();

        filter = findFilter(tState.groupId, tState.targetId);
        target = findTarget(tState.groupId, tState.targetId);
        if (target == NULL) continue;
        if (filter == NULL) continue;
        if (target->getStateCount() == 0) {
            // 第一次获取目标的状态,直接保存,不做任何计算。
            target->addState(tState.state);
            filter->setState(tState.state.getData());
            continue;
        }
        //计算一步预测值
        Matrix matrix = filter->estimate();
        // 根据测量值更新预测数据
        filter->updateByMeasure(tState.state.getData());
        State s;
        s.setData(matrix);
        s.setTime(tState.time);
        target->addState(s);
    }

    //项目验收后去掉
    initMessage();

    printTargetGroups();
}
예제 #21
0
파일: widget.cpp 프로젝트: JuarezASF/Code
vector<vector<Point> > Widget::predictFuture(int futureSize){
    vector<vector<Point> > future;
    future.resize(targets.size());

    for(unsigned int i = 0; i < targets.size(); i++)
        {
        KalmanFilter *KF;
        KF = targets[i]->KF;

        KalmanFilter DelfusOracle = myMath::copyKF(*KF);
            /*para ver o futuro copiamos o estado do filtro atual e
              o realimentamos com suas próprias previsões um número fixo de vezes*/
        for(int j = 0; j < futureSize; j++)
                //CALCULA PONTOS DO FUTURO
            {

            Mat prediction = DelfusOracle.predict();
            Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

            future[i].push_back(predictPt);

            Mat_<float> predictedMeasurement(2,1);
            predictedMeasurement(0) = prediction.at<float>(0);
            predictedMeasurement(1) = prediction.at<float>(1);

            DelfusOracle.correct(predictedMeasurement);

            DelfusOracle.errorCovPre.copyTo(DelfusOracle.errorCovPost);
                //copiamos covPre para covPost seguindo a dica de um fórum
                //o Vidal não gosta dessa dica, diz ele que isso engana o filtro
            }


        }//end for each color

    return future;
}
/*
 * box_update
 * Updates the box that we are going to search for the template
 * match in to be centered over the center of the current match
 * location.
 *
 * Inputs:
 * 		kal -> reference of a kalman filter object that is used to track and predict the match location
 * 		bb 	-> rectangle that is the size of the training image used to find the best point and is in the location of the match
 *
 * Outputs:
 * 		measurement -> the point to store the measured point
 * 		ctr_point -> the center point of the best match.
 * 		kal_point -> the center point adjusted by the kalman filter
 */
void box_update(KalmanFilter &kal, Rect &bb, Mat_<float> &measurement, Point2f &ctr_point, Point2f &kal_point){
	Point measp = Point(bb.tl().x + (bb.width / 2), bb.tl().y + (bb.height / 2));
	//save measured matrix
	measurement(0) = measp.x;
	measurement(1) = measp.y;

	ctr_point = measp; //save measured center point

	Mat estimated = kal.correct(measurement); //correct the predicted center with the measurement point
	Point statePt(estimated.at<float>(0),estimated.at<float>(1)); //the corrected center
	kal_point = statePt; //save the corrected point
	//update selection so next time we have the mask in the right spot
	selection.x = statePt.x - (bb.width/2);
	selection.y = statePt.y - (bb.height/2);
	selection.width = bb.width;
	selection.height = bb.height;
}
예제 #23
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;
}
예제 #24
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);

 */




}
예제 #25
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());
}
예제 #26
0
Point kalmanPredict() 
{
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
    return predictPt;
}
예제 #27
0
  TEST(KalmanFilterTest, validationEffect)
  {  
    KalmanFilter kf;

    MatrixXd A(1,1); A << 1;
    kf.setStateTransitionModel(A);
    MatrixXd H(1,1); H << 1;
    kf.setObservationModel(H);
    MatrixXd Q(1,1); Q << 0.1;
    kf.setProcessNoiseCovariance(Q);
    MatrixXd R(1,1); R << 10;
    kf.setMeasurementNoiseCovariance(R);

    // validate has an effect?
    InputValue measurement(1);
    Input in(measurement);
    Output out;
  
    EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error);	// "not yet validated"
  
    EXPECT_NO_THROW(kf.validate());

    // changing a parameter -> KF must be re-validated
    kf.setProcessNoiseCovariance(Q);
    EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error);	// "not yet validated"
    EXPECT_NO_THROW(kf.validate());
  
    // KF should now be released and return an estimate (should not be
    // the default)
    EXPECT_NO_THROW(out = kf.estimate(in));
    EXPECT_GT(out.size(), 0);
    EXPECT_EQ(kf.getState().size(), out.size());
  }
예제 #28
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);
}
예제 #29
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));
}
예제 #30
0
void CHumanTracker::detectAndTrackFace()
{
    static ros::Time probe;

	// Do ROI
	debugFrame = rawFrame.clone();
	Mat img =  this->rawFrame(searchROI);

	faces.clear();
	ostringstream txtstr;
    const static Scalar colors[] =  { CV_RGB(0,0,255),
        CV_RGB(0,128,255),
        CV_RGB(0,255,255),
        CV_RGB(0,255,0),
        CV_RGB(255,128,0),
        CV_RGB(255,255,0),
        CV_RGB(255,0,0),
        CV_RGB(255,0,255)} ;
    Mat gray;
    Mat frame( cvRound(img.rows), cvRound(img.cols), CV_8UC1 );
    cvtColor( img, gray, CV_BGR2GRAY );
    resize( gray, frame, frame.size(), 0, 0, INTER_LINEAR );
    //equalizeHist( frame, frame );

	// This if for internal usage
    const ros::Time _n = ros::Time::now();
	double dt = (_n - probe).toSec();
	probe = _n;


	CvMat _image = frame;

    if (!storage.empty())
    {
        cvClearMemStorage(storage);
    }
    CvSeq* _objects = cvHaarDetectObjects(&_image, cascade, storage,
			1.2, initialScoreMin, CV_HAAR_DO_CANNY_PRUNING|CV_HAAR_SCALE_IMAGE, minFaceSize, maxFaceSize);

	vector<CvAvgComp> vecAvgComp;
	Seq<CvAvgComp>(_objects).copyTo(vecAvgComp);

	// End of using C API

	isFaceInCurrentFrame = (vecAvgComp.size() > 0);

    // This is a hack
    bool isProfileFace = false;
    if ((profileHackEnabled) && (!isFaceInCurrentFrame) && ((trackingState == STATE_REJECT) || (trackingState == STATE_REJECT)))
    {
        ROS_DEBUG("Using Profile Face hack ...");

        if (!storageProfile.empty()) {
            cvClearMemStorage(storageProfile);
        }
        CvSeq* _objectsProfile = cvHaarDetectObjects(&_image, cascadeProfile, storageProfile,
                1.2, initialScoreMin, CV_HAAR_DO_CANNY_PRUNING|CV_HAAR_SCALE_IMAGE, minFaceSize, maxFaceSize);
        vecAvgComp.clear();
        Seq<CvAvgComp>(_objectsProfile).copyTo(vecAvgComp);
        isFaceInCurrentFrame = (vecAvgComp.size() > 0);
        if (isFaceInCurrentFrame)
        {
            ROS_DEBUG("The hack seems to work!");
        }
        isProfileFace = true;
    }

	if (trackingState == STATE_LOST)
	{
		if (isFaceInCurrentFrame)
		{
			stateCounter++;
			trackingState = STATE_DETECT;
		}
	}

	if (trackingState == STATE_DETECT)
	{
		if (isFaceInCurrentFrame)
		{
			stateCounter++;
		}
		else
		{
			stateCounter = 0;
			trackingState = STATE_LOST;
		}

		if (stateCounter > minDetectFrames)
		{
			stateCounter = 0;
			trackingState = STATE_TRACK;
		}

	}

	if (trackingState == STATE_TRACK)
	{
		if (!isFaceInCurrentFrame)
		{
			trackingState = STATE_REJECT;
		}
	}

	if (trackingState == STATE_REJECT)
	{
		float covNorm = sqrt(
                        pow(KFTracker.errorCovPost.at<float>(0,0), 2) +
                        pow(KFTracker.errorCovPost.at<float>(1,1), 2)
						);

		if (!isFaceInCurrentFrame)
		{
			stateCounter++;
		}
		else
		{
			stateCounter = 0;
			trackingState = STATE_TRACK;
		}

		if ((stateCounter > minRejectFrames) && (covNorm > maxRejectCov))
		{
			trackingState = STATE_LOST;
			stateCounter = 0;
			resetKalmanFilter();
            reset();
		}
	}

	if ((trackingState == STATE_TRACK) || (trackingState == STATE_REJECT))
	{
		bool updateFaceHist = false;
		// This is important:

        KFTracker.transitionMatrix.at<float>(0,2) = dt;
        KFTracker.transitionMatrix.at<float>(1,3) = dt;

        Mat pred = KFTracker.predict();

		if (isFaceInCurrentFrame)
		{
            //std::cout << vecAvgComp.size() << " detections in image " << std::endl;
			float minCovNorm = 1e24;
			int i = 0;
			for( vector<CvAvgComp>::const_iterator rr = vecAvgComp.begin(); rr != vecAvgComp.end(); rr++, i++ )
			{
				copyKalman(KFTracker, MLSearch);
				CvRect r = rr->rect;
				r.x += searchROI.x;
				r.y += searchROI.y;
				double nr = rr->neighbors;
				Point center;
				Scalar color = colors[i%8];

				float normFaceScore = 1.0 - (nr / 40.0);
				if (normFaceScore > 1.0) normFaceScore = 1.0;
				if (normFaceScore < 0.0) normFaceScore = 0.0;
                setIdentity(MLSearch.measurementNoiseCov, Scalar_<float>::all(normFaceScore));

				center.x = cvRound(r.x + r.width*0.5);
				center.y = cvRound(r.y + r.height*0.5);

				measurement.at<float>(0) = r.x;
				measurement.at<float>(1) = r.y;
				measurement.at<float>(2) = r.width;
				measurement.at<float>(3) = r.height;

                MLSearch.correct(measurement);

				float covNorm = sqrt(
                    pow(MLSearch.errorCovPost.at<float>(0,0), 2) +
                    pow(MLSearch.errorCovPost.at<float>(1,1), 2)
				);

				if (covNorm < minCovNorm)
				{
					minCovNorm = covNorm;
					MLFace = *rr;
				}

//                if ((debugLevel & 0x02) == 0x02)
//                {
                    rectangle(debugFrame, center - Point(r.width*0.5, r.height*0.5), center + Point(r.width*0.5, r.height * 0.5), color);

                    txtstr.str("");
                    txtstr << "   Sc:" << rr->neighbors << " S:" << r.width << "x" << r.height;

                    putText(debugFrame, txtstr.str(), center, FONT_HERSHEY_PLAIN, 1, color);
//                }
			}

			// TODO: I'll fix this shit
            Rect r(MLFace.rect);
			r.x += searchROI.x;
			r.y += searchROI.y;
			faces.push_back(r);
			double nr = MLFace.neighbors;
			faceScore = nr;
            if (isProfileFace) faceScore = 0.0;
			float normFaceScore = 1.0 - (nr / 40.0);
			if (normFaceScore > 1.0) normFaceScore = 1.0;
			if (normFaceScore < 0.0) normFaceScore = 0.0;
            setIdentity(KFTracker.measurementNoiseCov, Scalar_<float>::all(normFaceScore));
			measurement.at<float>(0) = r.x;
			measurement.at<float>(1) = r.y;
			measurement.at<float>(2) = r.width;
			measurement.at<float>(3) = r.height;
            KFTracker.correct(measurement);

            // We see a face
			updateFaceHist = true;
		}
		else
		{
            KFTracker.statePost = KFTracker.statePre;
            KFTracker.errorCovPost = KFTracker.errorCovPre;
		}

        // TODO: MOVE THIS
        for (unsigned int k = 0; k < faces.size(); k++) {
            rectangle(debugFrame, faces.at(k), CV_RGB(128, 128, 128));
        }

        beleif.x = max<int>(KFTracker.statePost.at<float>(0), 0);
        beleif.y = max<int>(KFTracker.statePost.at<float>(1), 0);
        beleif.width = min<int>(KFTracker.statePost.at<float>(4), iWidth - beleif.x);
        beleif.height = min<int>(KFTracker.statePost.at<float>(5), iHeight - beleif.y);

		Point belCenter;
		belCenter.x = beleif.x + (beleif.width * 0.5);
		belCenter.y = beleif.y + (beleif.height * 0.5);

        if ((debugLevel & 0x02) == 0x02)
        {
            txtstr.str("");
//            txtstr << "P:" << std::setprecision(3) << faceUncPos << " S:" << beleif.width << "x" << beleif.height;
//            putText(debugFrame, txtstr.str(), belCenter + Point(0, 50), FONT_HERSHEY_PLAIN, 2, CV_RGB(255,0,0));

//            circle(debugFrame, belCenter, belRad, CV_RGB(255,0,0));
//            circle(debugFrame, belCenter, (belRad - faceUncPos < 0) ? 0 : (belRad - faceUncPos), CV_RGB(255,255,0));
//            circle(debugFrame, belCenter, belRad + faceUncPos, CV_RGB(255,0,255));
        }

        //searchROI.x = max<int>(belCenter.x - KFTracker.statePost.at<float>(4) * 2, 0);
        //searchROI.y = max<int>(belCenter.y - KFTracker.statePost.at<float>(5) * 2, 0);
        //int x2 = min<int>(belCenter.x + KFTracker.statePost.at<float>(4) * 2, iWidth);
        //int y2 = min<int>(belCenter.y + KFTracker.statePost.at<float>(4) * 2, iHeight);
		//searchROI.width = x2 - searchROI.x;
		//searchROI.height = y2 - searchROI.y;


		if ((updateFaceHist) && (skinEnabled))
		{
            //updateFaceHist is true when we see a real face (not all the times)
            Rect samplingWindow;
//            samplingWindow.x = beleif.x + (0.25 * beleif.width);
//            samplingWindow.y = beleif.y + (0.1 * beleif.height);
//            samplingWindow.width = beleif.width * 0.5;
//            samplingWindow.height = beleif.height * 0.9;
            samplingWindow.x = measurement.at<float>(0) + (0.25 * measurement.at<float>(2));
            samplingWindow.y = measurement.at<float>(1) + (0.10 * measurement.at<float>(3));
            samplingWindow.width = measurement.at<float>(2) * 0.5;
            samplingWindow.height = measurement.at<float>(3) * 0.9;
            if ((debugLevel & 0x04) == 0x04)
            {
                rectangle(debugFrame, samplingWindow, CV_RGB(255,0,0));
            }
			Mat _face = rawFrame(samplingWindow);
			generateRegionHistogram(_face, faceHist);
		}


	}


//    if ((debugLevel & 0x02) == 0x02)
//    {
//        rectangle(debugFrame, searchROI, CV_RGB(0,0,0));
//        txtstr.str("");
//        txtstr << strStates[trackingState] << "(" << std::setprecision(3) << (dt * 1e3) << "ms )";
//        putText(debugFrame, txtstr.str() , Point(30,300), FONT_HERSHEY_PLAIN, 2, CV_RGB(255,255,255));
//    }

//	dt =  ((double) getTickCount() - t) / ((double) getTickFrequency()); // In Seconds
}