C++ KalmanFilter类代码示例

/**
 * @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);
    }
}

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;

}

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;
}

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;
    }
}

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;
}

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;
}

// 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));
	}
}

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;

}

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;
}

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;
    }
}

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();
}

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;
}