OpenCV 卡尔曼滤波器

Question

我有三个陀螺仪值，俯仰、滚动和偏航。我想添加卡尔曼滤波器以获得更准确的值。我找到了实现卡尔曼滤波器的opencv库，但我不明白它是如何工作的。

你能给我任何可以帮助我的帮助吗？我在互联网上没有找到任何相关的主题。

我试图让它在一个轴上工作。

const float A[] = { 1, 1, 0, 1 };
CvKalman* kalman;
CvMat* state = NULL;
CvMat* measurement;

void kalman_filter(float FoE_x, float prev_x)
{
    const CvMat* prediction = cvKalmanPredict( kalman, 0 );
    printf("KALMAN: %f %f %f\n" , prev_x, prediction->data.fl[0] , prediction->data.fl[1] );
    measurement->data.fl[0] = FoE_x;
    cvKalmanCorrect( kalman, measurement);
}

主要

kalman = cvCreateKalman( 2, 1, 0 );
state = cvCreateMat( 2, 1, CV_32FC1 );
measurement = cvCreateMat( 1, 1, CV_32FC1 );
cvSetIdentity( kalman->measurement_matrix,cvRealScalar(1) );
memcpy( kalman->transition_matrix->data.fl, A, sizeof(A));
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(2.0) );
cvSetIdentity(kalman->measurement_noise_cov, cvRealScalar(3.0));
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1222));
kalman->state_post->data.fl[0] = 0;

当我从陀螺仪接收数据时，我每次都会调用它：

kalman_filter(prevr, mpe->getGyrosDegrees().roll);

我认为在 kalman_filter 中，第一个参数是前一个值，第二个是当前值。我不是，这段代码不起作用......我知道我有很多工作要做，但我不知道如何继续，改变什么......

Answer 1

您似乎为协方差矩阵赋予了过高的值。

kalman->process_noise_cov 是'过程噪声covariance matrix'，它在卡尔曼文献中经常被称为Q。值越低，结果就越平滑。

kalman->measurement_noise_cov 是'测量噪声协方差矩阵'，它在卡尔曼文献中经常被称为R。值越高，结果就越平滑。

这两个矩阵之间的关系定义了您正在执行的过滤的数量和形状。

如果Q 的值很高，则意味着您正在测量的信号变化很快，您需要滤波器具有适应性。如果它很小，那么大的变化将归因于测量中的噪声。

如果R 的值较高（与Q 相比），则表明测量有噪声，因此将对其进行更多过滤。

尝试使用较低的值，例如 q = 1e-5 和 r = 1e-1，而不是 q = 2.0 和 r = 3.0。