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。

OpenCV卡尔曼滤波器

1 个答案: