Bộ lọc OpenCV Kalman

Question

Tôi có ba giá trị con quay hồi chuyển, độ dốc, cuộn và ngáp. Tôi muốn thêm bộ lọc Kalman để có được các giá trị chính xác hơn. Tôi tìm thấy thư viện opencv, mà thực hiện một bộ lọc Kalman, nhưng tôi không thể hiểu nó như thế nào là nó thực sự làm việc.

Bạn có thể cho tôi bất kỳ trợ giúp nào có thể giúp tôi không? Tôi không tìm thấy bất kỳ chủ đề liên quan nào trên internet.

Tôi đã cố gắng làm cho nó hoạt động với một trục.

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

trong chính

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; 

Và tôi gọi mọi này, khi tôi nhận dữ liệu từ gyro:

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

Tôi nghĩ trong kalman_filter tham số đầu tiên là giá trị trước đó và lần thứ hai là giá trị dòng. Tôi không và mã này không hoạt động ... Tôi biết tôi có rất nhiều công việc với nó, nhưng tôi không biết làm thế nào để tiếp tục, những gì để thay đổi ...

Answer 1

Có vẻ như bạn đưa ra các giá trị quá cao cho ma trận hiệp phương sai.

kalman->process_noise_cov là 'trình tiếng ồn covariance matrix' và nó thường được gọi trong các tài liệu Kalman như Q. Kết quả sẽ mượt mà hơn với các giá trị thấp hơn.

kalman->measurement_noise_cov là ma trận hiệp phương sai đo lường 'đo lường' và nó thường được đề cập trong tài liệu Kalman là R. Kết quả sẽ mượt mà hơn với các giá trị cao hơn.

Mối quan hệ giữa hai ma trận đó xác định số lượng và hình dạng lọc bạn đang thực hiện.

Nếu giá trị của Q cao, điều đó có nghĩa là tín hiệu bạn đang đo thay đổi nhanh chóng và bạn cần bộ lọc có khả năng thích ứng. Nếu nó là nhỏ, thì các biến thể lớn sẽ được quy cho tiếng ồn trong đo lường.

Nếu giá trị của R cao (so với Q), nó sẽ cho biết rằng việc đo nhiễu sẽ được lọc nhiều hơn.

Thử các giá trị thấp hơn như q = 1e-5 và r = 1e-1 thay vì q = 2.0 và r = 3.0.