“I am trying to convert the Kalman filter example from OpenCV to a 2D implementation, assuming the point is moving along the bisector. But when running the code, you can see that the video output is far from optimal. Can you tell me where I am going wrong?”

`

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"
#include <stdio.h>
#include <iostream>
using namespace cv;
using namespace std;

static inline Point calcPoint(Point2f center, double R, double angle)
{
    return center + Point2f((float)cos(angle), (float)-sin(angle)) * (float)R;
}
/*********************************************************************************************/
/*********************************************************************************************/
static inline Point my_calcPoint(Point2f center, double x, double y)
{
    return center + Point2f(x, y);
}
/*********************************************************************************************/
static void help()
{
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"
#include <stdio.h>
#include <iostream>
using namespace cv;
using namespace std;

static inline Point my_calcPoint(Point2f center, double x, double y)
{
    return center + Point2f(x, y);
}
static void help()
{
    printf("n Test kalman predict future point n");
}
int main(int, char **)
{


    // my version for point moving in 2d dimension
    Mat my_img(500, 500, CV_8UC3);
    KalmanFilter my_KF(4, 2, 0);
    Mat my_state(4, 1, CV_32F); /* (x, delta_x, y, delta_y) */
    Mat my_processNoise(4, 1, CV_32F);
    Mat my_measurement = Mat::zeros(2, 1, CV_32F);
    
    char code = (char)-1;
    for (;;)
    {

        my_state.at<float>(0) = 0; 
        my_state.at<float>(1) = 0.2;  
        my_state.at<float>(2) = 0;        
        my_state.at<float>(3) = 0.2f;        

        my_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1);
        my_KF.measurementMatrix = (Mat_<float>(2, 4) << 1, 0, 0, 0, 0, 0, 1, 0);
        setIdentity(my_KF.processNoiseCov, Scalar::all(1e-5));
        setIdentity(my_KF.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(my_KF.errorCovPost, Scalar::all(1));
        randn(my_KF.statePost, Scalar::all(0), Scalar::all(0.1));
        double x = 0;
        double y = 0;


        for (;;)
        {

            Point2f my_center(my_img.cols * 0.5f, my_img.rows * 0.5f);
            double my_stateAngle = my_state.at<float>(0);
            x = x + 0.5;
            y = y + 0.5;
            Point my_statePt = my_calcPoint(my_center, x, y);
            Mat my_prediction = my_KF.predict();
            double my_predict_x = my_prediction.at<float>(0);
            double my_predict_y = my_prediction.at<float>(1);
            Point my_predictPt = my_calcPoint(my_center, my_predict_x, my_predict_y);

            randn(my_measurement, Scalar::all(0), Scalar::all(my_KF.measurementNoiseCov.at<float>(0))); // Dubbio: Questo va aggiunto solo qua ?
            // generate measurement
            my_measurement += my_KF.measurementMatrix * my_state; 
            double my_meas_x = my_measurement.at<float>(0);
            double my_meas_y = my_measurement.at<float>(1);
            Point my_measPt = my_calcPoint(my_center, my_meas_x, my_meas_y);

// plot points
#define drawCross(my_center, my_color, my_d)                                   
    line(my_img, Point(my_center.x - my_d, my_center.y - my_d),                   
         Point(my_center.x + my_d, my_center.y + my_d), my_color, 1, LINE_AA, 0); 
    line(my_img, Point(my_center.x + my_d, my_center.y - my_d),                   
         Point(my_center.x - my_d, my_center.y + my_d), my_color, 1, LINE_AA, 0)

            my_img = Scalar::all(0);
            drawCross(my_statePt, Scalar(255, 255, 255), 3);
            drawCross(my_measPt, Scalar(0, 0, 255), 3);
            drawCross(my_predictPt, Scalar(0, 255, 0), 3);
            line(my_img, my_statePt, my_measPt, Scalar(0, 0, 255), 3, LINE_AA, 0);
            line(my_img, my_statePt, my_predictPt, Scalar(0, 255, 255), 3, LINE_AA, 0);
            if (theRNG().uniform(0, 4) != 0)
            my_KF.correct(my_measurement);
            randn(my_processNoise, Scalar(0), Scalar::all(sqrt(my_KF.processNoiseCov.at<float>(0, 0))));
            my_state = my_KF.transitionMatrix * my_state + my_processNoise;
            imshow("my_Kalman", my_img);


            code = (char)waitKey(100);
            if (code > 0)
                break;
        }
        if (code == 27 || code == 'q' || code == 'Q')
            break;
    }
    return 0;
}

`

Thanks for your help help help help help help help help help help help help help!