//   Tracking of rotating point.

//   Rotation speed is constant.

//   Both state and measurements vectors are 1D (a point angle),

//   Measurement is the real point angle + gaussian noise.

//   The real and the estimated points are connected with yellow line segment,

//   the real and the measured points are connected with red line segment.

//   (if Kalman filter works correctly,

//    the yellow segment should be shorter than the red one).

//   Pressing any key (except ESC) will reset the tracking with a different speed.

//   Pressing ESC will stop the program.

#include "cv.h"

#include "highgui.h"

using namespace cv;

Point calc_point(Mat& img, double angle) {

    int x = round(img.cols/2 + img.rows/3*cos(angle));

    int y = round(img.cols/2 - img.rows/3*sin(angle));

    return Point(x, y);

};

void draw_cross(Mat& img, Point center, Scalar color, int d) {

    line(img, Point(center.x - d, center.y - d),

                 Point(center.x + d, center.y + d), color, 1, CV_AA, 0);

    line(img, Point(center.x + d, center.y - d),

                 Point(center.x - d, center.y + d), color, 1, CV_AA, 0);

};

int main (int argc, char * const argv[]) {

    Mat img = Mat(500, 500, CV_8UC3);

    KalmanFilter kalman = KalmanFilter(2, 1, 0);

    Mat state = Mat(2, 1, CV_32FC1);  // (phi, delta_phi)

    Mat process_noise = Mat(2, 1, CV_32FC1);

    Mat measurement = Mat(1, 1, CV_32FC1);

    Mat measurement_noise = Mat(1, 1, CV_32FC1);

    measurement.setTo(Scalar(0));

    namedWindow("kalman", 1);

    float m[2][2] = {{1, 1}, {0, 1}};

    kalman.transitionMatrix = Mat(2, 2, CV_32FC1, m);

    setIdentity(kalman.measurementMatrix, Scalar(1));

    setIdentity(kalman.processNoiseCov, Scalar(1e-5));

    setIdentity(kalman.measurementNoiseCov, Scalar(1e-1));

    setIdentity(kalman.errorCovPost, Scalar(1));

    int code = -1;

    while (true) {

        randu(state, Scalar(0), Scalar(0.1));

        randu(kalman.statePost, Scalar(0), Scalar(0.1));

        while (true) {

            img.setTo(Scalar(0));

            float state_angle = state.at<float>(0,0);

            Point state_pt = calc_point(img, state_angle);

            // predict a measurement

            Mat prediction = kalman.predict();

            float predict_angle = prediction.at<float>(0, 0);

            Point predict_pt = calc_point(img, predict_angle);

            // generate measurement with measurement noise

            randn(measurement_noise, Scalar(0), Scalar(sqrt(kalman.measurementNoiseCov.at<float>(0, 0))));

            measurement = kalman.measurementMatrix * state + measurement_noise;

            float measurement_angle = measurement.at<float>(0, 0);

            Point measurement_pt = calc_point(img, measurement_angle);

            // draw stuff

            line(img, state_pt, measurement_pt, CV_RGB(255, 0,0), 3,  CV_AA, 0);

            line(img, state_pt, predict_pt, CV_RGB(255, 255, 0), 3,  CV_AA, 0);

            draw_cross(img, state_pt, CV_RGB(255, 255, 255), 3);

            draw_cross(img, measurement_pt, CV_RGB(255, 0,0), 3);

            draw_cross(img, predict_pt, CV_RGB(0, 255, 0), 3);

            // correct kalman internal state with measurement

            kalman.correct(measurement);

            // update state

            randn(process_noise, Scalar(0), Scalar(sqrt(kalman.processNoiseCov.at<float>(0, 0))));

            state = kalman.transitionMatrix * state + process_noise;

            imshow("kalman", img);

            code = waitKey(100);

            if (code > -1)

                break;

        };

        if (code == 27 || code == 113 || code == 65617)

            break;

    };

};