if (contour_index >= 0) {
    // Moments

    cv::Moments moments = cv::moments(contours[contour_index], true);

    double marker_y = (int)(moments.m01 / moments.m00);

    double marker_x = (int)(moments.m10 / moments.m00);



    // Measurements

    cv::Mat measurement = (cv::Mat1f(2, 1) << marker_x, marker_y);



    // Correction

    cv::Mat estimated = kalman.correct(measurement);



    // Show result

    cv::Rect rect = cv::boundingRect(contours[contour_index]);

    cv::rectangle(image, rect, cv::Scalar(0, 255, 0));

}



// Prediction

cv::Mat1f prediction = kalman.predict();

int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);



// Calculate object heading fraction

float heading = -((image.cols/2)-prediction(0, 0))/(image.cols/2);

sprintf(textBuffer, "heading = %+3.2f", heading);

putText(image, textBuffer, cvPoint(30,30), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);



// Sample the object color

if(learnMode) {



    // Crosshairs

    cv::line(image, cvPoint(image.cols/2, 0), cvPoint(image.cols/2, image.rows/2 - 2), green); //top vertical crosshair

    cv::line(image, cvPoint(image.cols/2, image.rows/2 + 2), cvPoint(image.cols/2, image.rows), green); //bottom vertical crosshair

    cv::line(image, cvPoint(0, image.rows/2), cvPoint(image.cols/2 - 2, image.rows/2), green); //left horizontal crosshair

    cv::line(image, cvPoint(image.cols/2 + 2, image.rows/2), cvPoint(image.cols, image.rows/2), green); //right horizontal crosshair



    cv::Vec3b hsvSample = hsv.at<cv::Vec3b>(cvPoint(image.cols/2, image.rows/2));

    sprintf(textBuffer, "hsvSample = %3d, %3d, %3d", hsvSample[0], hsvSample[1], hsvSample[2]);

    putText(image, textBuffer, cvPoint(30,120), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);



    cv::setTrackbarPos("H max", "binalized", hsvSample[0]+20);

    cv::setTrackbarPos("H min", "binalized", hsvSample[0]-20);



    cv::setTrackbarPos("S max", "binalized", hsvSample[1]+20);

    cv::setTrackbarPos("S min", "binalized", hsvSample[1]-20);



    cv::setTrackbarPos("V max", "binalized", hsvSample[2]+20);

    cv::setTrackbarPos("V min", "binalized", hsvSample[2]-20);

}



// Show predicted position

cv::circle(image, cv::Point(prediction(0, 0), prediction(0, 1)), radius, green, 2);



//Speed

if ((key >= '0') && (key <= '9'))

{

    speed = (key-'0')*0.1;

    //printf("speed = %3.2fn", speed);

}

sprintf(textBuffer, "speed = %3.2f", speed);

putText(image, textBuffer, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

// Drone control

double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;



// Auto-follow

vx = speed;

vr = -heading;



// Manual control override

if (key == 0x260000) vx =  1.0;

if (key == 0x280000) vx = -1.0;

if (key == 0x250000) vr =  1.0;

if (key == 0x270000) vr = -1.0;

if (key == 'q')      vz =  1.0;

if (key == 'a')      vz = -1.0;

if (key == 'l')      learnMode = !learnMode;



ardrone.move3D(vx, vy, vz, vr);