OpenCV 2D Kalman Filter实现效果不佳，请求问题排查

问题背景 #

将OpenCV官方Kalman Filter示例改为2D实现，假设点沿角平分线移动，但视频输出效果未达预期。代码及输出截图如下：

代码实现 #

#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;
}

输出效果 #

问题排查与修复 #

1. 真实状态与卡尔曼跟踪状态完全脱节 #

你用x = x + 0.5; y = y + 0.5;手动更新显示的真实点，但卡尔曼滤波器跟踪的my_state是通过转移矩阵独立更新的，两者没有关联，导致预测点完全偏离真实路径。

• 修复：删除手动更新的x和y，直接从my_state中读取真实位置：

  // 替换原x、y手动更新代码
  double true_x = my_state.at<float>(0);
  double true_y = my_state.at<float>(2);
  Point my_statePt = my_calcPoint(my_center, true_x, true_y);
  

2. 卡尔曼初始状态与真实状态不匹配 #

初始化my_state为(0, 0.2, 0, 0.2)后，又用randn随机初始化卡尔曼的后验状态，导致初始阶段滤波器就偏离真实值。

• 修复：将卡尔曼初始状态直接设置为真实状态，去掉随机初始化：

  // 替换randn那一行
  my_KF.statePost = my_state.clone();
  

3. 测量值生成逻辑错误 #

先给my_measurement添加噪声再叠加真实测量值，逻辑顺序颠倒，导致噪声叠加异常。

• 修复：先计算真实测量值，再添加噪声：

  // 调整测量值生成顺序
  my_measurement = my_KF.measurementMatrix * my_state;
  randn(my_processNoise, Scalar::all(0), Scalar::all(sqrt(my_KF.measurementNoiseCov.at<float>(0))));
  my_measurement += my_processNoise;
  

4. 随机跳过修正步骤干扰收敛 #

if (theRNG().uniform(0, 4) != 0)会随机跳过1/4的修正步骤，调试阶段会破坏滤波器的收敛过程。

• 修复：注释掉随机判断，确保每次都执行修正：

  // 直接执行修正
  my_KF.correct(my_measurement);
  

修复后的代码会让绿色预测点紧密跟踪白色真实点，红色噪声测量点被有效平滑，符合2D匀速运动的卡尔曼滤波预期效果。

内容的提问来源于stack exchange，提问作者Alfredo Miccichè