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是"过程噪声协方差矩阵 ",它经常在卡尔曼文献中被称为Q.较低的值会使结果更平滑.

kalman->measurement_noise_cov是"测量噪声协方差矩阵",它经常在卡尔曼文献中被称为R.较高的值会使结果更平滑.

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

如果值Q很高,则表示您测量的信号变化很快,您需要使滤波器具有适应性.如果它很小,那么大的变化将归因于测量中的噪声.

如果值为R高(Q与之相比),则表示测量值有噪声,因此将进行更多过滤.

尝试较低的值q = 1e-5,r = 1e-1而不是q = 2.0和r = 3.0.