void Kalman::filter(cv::Point2f center, cv::Mat &image_tracking)
{
std::ostringstream num_str;
cv::Mat_<float> measurement(2,1);
cv::Mat processNoise(4, 1, CV_32F);
//Kalman
int state_near_id = getStateIDNearest(center);
if(state_near_id <= -1)
{
return;
}
copy(kman, kman_prevs[state_near_id]);
kman.state(0) = center.x;
kman.state(1) = center.y;
//Prediction
cv::Mat prediction = kman.KF.predict();
//std::cout<<prediction <<" ";
cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
//generate measurement
randn( measurement, cv::Scalar::all(0),
cv::Scalar::all(kman.KF.measurementNoiseCov.at<float>(0)));
measurement += kman.KF.measurementMatrix*kman.state;
cv::Point measPt(measurement(0), measurement(1));
//Correct
cv::Mat estimated = kman.KF.correct(measurement);
cv::Point correctPt(estimated.at<float>(0),estimated.at<float>(1));
//num_str<< i <<"-p";
AMat::drawCross(predictPt, cv::Scalar(255, 0, 0), 1, image_tracking, num_str.str());
//num_str.str("");
//num_str<< i <<"-m";
AMat::drawCross(measPt, cv::Scalar(0, 255, 0), 1, image_tracking, num_str.str());
//num_str.str("");
//num_str<< i <<"-c";
AMat::drawCross(correctPt, cv::Scalar(0, 0, 255), 1, image_tracking, num_str.str());
//num_str.str("");
/*
for (int i = 0; i < (int)predicts.size()-1; i++) {
line(image_tracking, predicts[i], predicts[i+1], Scalar(255, 0, 0), 1);
}
for (int i = 0; i < (int)measures.size()-1; i++) {
line(image_tracking, measures[i], measures[i+1], Scalar(0,255,0), 1);
}
for (int i = 0; i < (int)corrects.size()-1; i++) {
line(image_tracking, corrects[i], corrects[i+1], Scalar(0, 0, 255), 1);
}
*/
//randn( processNoise, cv::Scalar(0),
// cv::Scalar::all(sqrt(kman.KF.processNoiseCov.at<float>(0, 0))));
// kman.state = kman.KF.transitionMatrix*kman.state + processNoise;
}
void CLMS::process()
{
switch (m_filterType) {
case LMS_NOISE:
processNoise();
break;
case LMS_INTERFERENCE:
processInterference();
break;
default:
#if defined(__WXMSW__) || defined(__WXGTK__) || defined(__WXMAC__)
wxLogError(wxT("Unknown filter type in LMS = %d"), m_filterType);
#elif defined(WIN32)
// No WIN32 logging yet
#else
::syslog(LOG_ERR, "Unknown filter type in LMS = %d", m_filterType);
#endif
break;
}
}
