OpenCV卡尔曼滤波器 [英] OpenCV Kalman filter

问题描述

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

I have three gyroscope values, pitch, roll and yaw. I would like to add Kalman filter to get more accurate values. I found the opencv library, which implements a Kalman filter, but I can't understand it how is it really work.

您能给我任何可以帮助我的帮助吗?我在互联网上找不到任何相关主题.

Could you give me any help which can help me? I didn't find any related topics on the internet.

我试图使其在一个轴上起作用.

I tried to make it work for one axis.

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;

当我从陀螺仪接收数据时，我每次都这样称呼它:

And I call this everytime, when I receive data from gyro:

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

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

I thought in kalman_filter the first parameter is the previous value and the second is the currect value. I'm not and this code doesn't work... I know I have a lot of work with it, but I don't know how to continue, what to change...

推荐答案

似乎您给协方差矩阵赋予的值太高.

It seems like you are giving too high values to the covariance matrices.

kalman-> process_noise_cov 是'过程噪声协方差矩阵'，在卡尔曼文献中通常将其称为 Q .较低的值将使结果更平滑.

kalman->process_noise_cov is the 'process noise covariance matrix' and it is often referred in the Kalman literature as Q. The result will be smoother with lower values.

kalman-> measurement_noise_cov 是测量噪声协方差矩阵" ，在卡尔曼文献中通常将其称为 R .值越高，结果越平滑.

kalman->measurement_noise_cov is the 'measurement noise covariance matrix' and it is often referred in the Kalman literature as R. The result will be smoother with higher values.

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

The relation between those two matrices defines the amount and shape of filtering you are performing.

如果 Q 的值很高，则意味着您正在测量的信号变化很快，并且您需要使滤波器适应性强.如果很小，则测量值中的噪声将导致较大的差异.

If the value of Q is high, it will mean that the signal you are measuring varies quickly and you need the filter to be adaptable. If it is small, then big variations will be attributed to noise in the measure.

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

If the value of R is high (compared to Q), it will indicate that the measuring is noisy so it will be filtered more.

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

Try lower values like q = 1e-5 and r = 1e-1 instead of q = 2.0 and r = 3.0.

这篇关于OpenCV卡尔曼滤波器的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！