/*

   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>

#include <cmath>

using namespace std;

using namespace cv;

int main(int argc, char** argv)

{

    const float A[] = { 1, 1, 0, 1 };

    Mat img = Mat( Size(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::zeros( 1, 1, CV_32FC1 );

    RNG rng = RNG(-1);

    char code = -1;

    namedWindow( "Kalman", 1 );

    for(;;)

    {

        rng.fill(state,RNG::NORMAL,Scalar(0),Scalar(0.1));

        memcpy( kalman.transitionMatrix.data, A, sizeof(float)*sizeof(A));

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

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

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

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

        rng.fill(kalman.statePost, RNG::NORMAL, Scalar(0.0), Scalar(0.1) );

        for(;;)

        {

            #define calc_point(angle)                                      \

                Point( cvRound(img.cols/2 + img.cols/3*cos(angle)),  \

                         cvRound(img.cols/2 - img.cols/3*sin(angle)))

            float state_angle = state.ptr<float>(0)[0];

            Point state_pt = calc_point(state_angle);

            const Mat prediction = kalman.predict();

            float predict_angle = prediction.ptr<float>(0)[0];

            Point predict_pt = calc_point(predict_angle);

            float measurement_angle;

            Point measurement_pt;

            rng.fill(measurement, RNG::NORMAL, Scalar(0),

                       Scalar(sqrt(kalman.measurementNoiseCov.ptr<float>(0)[0])) );

            /* generate measurement */

            measurement=kalman.measurementMatrix*state+measurement;

            measurement_angle = measurement.ptr<float>(0)[0];

            measurement_pt = calc_point(measurement_angle);

            /* plot points */

            #define draw_cross( center, color, 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 )

            img.setTo(Scalar(0));

            draw_cross( state_pt, Scalar(255,255,255), 3 );

            draw_cross( measurement_pt, Scalar(255,0,0), 3 );

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

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

            kalman.correct( measurement );

            rng.fill(process_noise, RNG::NORMAL, Scalar(0),

                       Scalar(sqrt(kalman.processNoiseCov.ptr<float>(0)[0])));

            state=kalman.transitionMatrix*state+process_noise;

            imshow( "Kalman", img );

            code = (char) waitKey( 100 );

            if( code > 0 )

                break;

        }

        if( code == 27 || code == 'q' || code == 'Q' )

            break;

    }

    return 0;

}